ClutteredSceneGeneratorWidgetController.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    ArmarXSimulation::gui-plugins::ClutteredSceneGeneratorWidgetController
 * \author     Patrick Hegemann ( patrick dot hegemann at kit dot edu )
 * \date       2021
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ClutteredSceneGeneratorWidgetController.h"
 
#include <algorithm>
#include <filesystem>
#include <iterator>
#include <optional>
 
#include <glob.h>
 
#include <boost/algorithm/string/predicate.hpp>
 
#include <QFlags>
#include <QListWidget>
#include <qnamespace.h>
 
#include "ArmarXCore/core/PackagePath.h"
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include "ArmarXSimulation/libraries/ClutteredSceneGenerator/objects/SimulatedObject.h"
#include <ArmarXSimulation/libraries/ClutteredSceneGenerator/ClutteredSceneGenerator.h>
 
namespace armarx
{
    ClutteredSceneGeneratorWidgetController::ClutteredSceneGeneratorWidgetController()
    {
        widget_.setupUi(getWidget());
 
        // TODO(patrick.hegemann): Refactor
        // armarx::CMakePackageFinder finder("ArmarXSimulation");
        // std::filesystem::path objectSetsPath = finder.getDataDir();
        // objectSetsPath /= "ArmarXSimulation/random_objects";
        // ARMARX_INFO << "Finding object sets in " << objectSetsPath;
        // QString pathText(objectSetsPath.c_str());
        // widget_.lineEditObjectsOutputPath->setText(pathText);
        // widget_.lineEditObjectsInputPath->setText(pathText);
 
        refreshSets();
 
        updateObjectPoseConstraints();
    }
 
    ClutteredSceneGeneratorWidgetController::~ClutteredSceneGeneratorWidgetController() = default;
 
    void
    ClutteredSceneGeneratorWidgetController::loadSettings(QSettings* settings)
    {
    }
 
    void
    ClutteredSceneGeneratorWidgetController::saveSettings(QSettings* settings)
    {
    }
 
    void
    ClutteredSceneGeneratorWidgetController::onInitComponent()
    {
        usingProxy(simulatorProxyName_);
    }
 
    void
    ClutteredSceneGeneratorWidgetController::onConnectComponent()
    {
        // Scene generation tab
        connect(widget_.pushButtonRefreshObjectSets,
                &QPushButton::clicked,
                this,
                &ClutteredSceneGeneratorWidgetController::refreshSets);
        connect(widget_.pushButtonGenerateNewScene,
                &QPushButton::clicked,
                this,
                &ClutteredSceneGeneratorWidgetController::generateScene);
 
        connect(widget_.checkBoxAutoPoseConstraints,
                &QCheckBox::stateChanged,
                this,
                &ClutteredSceneGeneratorWidgetController::updateObjectPoseConstraints);
        connect(widget_.spinBoxTableX,
                QOverload<double>::of(&QDoubleSpinBox::valueChanged),
                this,
                &ClutteredSceneGeneratorWidgetController::updateObjectPoseConstraints);
        connect(widget_.spinBoxTableY,
                QOverload<double>::of(&QDoubleSpinBox::valueChanged),
                this,
                &ClutteredSceneGeneratorWidgetController::updateObjectPoseConstraints);
        connect(widget_.spinBoxTableZ,
                QOverload<double>::of(&QDoubleSpinBox::valueChanged),
                this,
                &ClutteredSceneGeneratorWidgetController::updateObjectPoseConstraints);
        connect(widget_.spinBoxTableW,
                QOverload<double>::of(&QDoubleSpinBox::valueChanged),
                this,
                &ClutteredSceneGeneratorWidgetController::updateObjectPoseConstraints);
        connect(widget_.spinBoxTableH,
                QOverload<double>::of(&QDoubleSpinBox::valueChanged),
                this,
                &ClutteredSceneGeneratorWidgetController::updateObjectPoseConstraints);
        connect(widget_.spinBoxTableD,
                QOverload<double>::of(&QDoubleSpinBox::valueChanged),
                this,
                &ClutteredSceneGeneratorWidgetController::updateObjectPoseConstraints);
 
        simulator_ = getProxy<SimulatorInterfacePrx>(simulatorProxyName_);
    }
 
    void
    ClutteredSceneGeneratorWidgetController::generateObjects()
    {
    }
 
    std::map<std::string, simulation::scene_generation::ObjectSet>
    ClutteredSceneGeneratorWidgetController::getObjectSets(const armarx::PackagePath& packagePath)
    {
        const auto& directoryPackagePath = packagePath.serialize();
        std::map<std::string, simulation::scene_generation::ObjectSet> sets;
 
        // TODO(patrick.hegemann): Refactor
        ARMARX_INFO << "Finding objects in " << packagePath.toSystemPath();
 
        for (const auto& setEntry : std::filesystem::directory_iterator(packagePath.toSystemPath()))
        {
            if (!setEntry.is_directory())
            {
                continue;
            }
            const std::string setName = setEntry.path().filename();
 
            std::vector<simulation::scene_generation::ObjectSource> objectSources;
            for (const auto& objectEntry : std::filesystem::directory_iterator(setEntry.path()))
            {
                if (objectEntry.path().extension().string() == ".xml")
                {
                    std::filesystem::path newPath(directoryPackagePath.path);
                    newPath /= setName / objectEntry.path().filename();
                    PackagePath objectPackagePath(directoryPackagePath.package, newPath);
 
                    simulation::scene_generation::ObjectSource objectSource = {
                        .path = objectPackagePath,
                        .type = simulation::scene_generation::SimulatedObjectType::AsRobot,
                        .objectID = std::nullopt};
                    objectSources.push_back(objectSource);
                }
            }
            simulation::scene_generation::ObjectSet currentObjectSet{.objects = objectSources};
 
            sets[setName] = currentObjectSet;
            ARMARX_INFO << "added set " << setName;
        }
 
        return sets;
    }
 
    void
    ClutteredSceneGeneratorWidgetController::refreshSets()
    {
        // boost::filesystem::path path(widget_.lineEditObjectsInputPath->text().toStdString());
        PackagePath path("ArmarXSimulation", "random_objects");
        objectSets_ = getObjectSets(path);
 
        // Clear list and delete all old items
        widget_.listWidgetObjectSets->clear();
        objectSetListItems_.clear();
        for (auto& item : objectSetListItems_)
        {
            delete item;
        }
 
        // Fill list widget
        for (const auto& [setName, _] : objectSets_)
        {
            auto* item = new QListWidgetItem;
            objectSetListItems_.push_back(item);
 
            // TODO(patrick.hegemann): Find a standard method to get the name of a directory
            std::string basename = setName.substr(setName.find_last_of('/') + 1);
            item->setText(QString::fromStdString(basename));
            item->setData(1, QString::fromStdString(setName));
            item->setFlags((item->flags() | Qt::ItemIsUserCheckable) & ~Qt::ItemIsSelectable);
            item->setCheckState(Qt::Checked);
            widget_.listWidgetObjectSets->addItem(item);
        }
    }
 
    void
    ClutteredSceneGeneratorWidgetController::updateObjectPoseConstraints()
    {
        if (widget_.checkBoxAutoPoseConstraints->isChecked())
        {
            const double tableX = widget_.spinBoxTableX->value();
            const double tableY = widget_.spinBoxTableY->value();
            const double tableZ = widget_.spinBoxTableZ->value();
            const double tableW = widget_.spinBoxTableW->value();
            const double tableH = widget_.spinBoxTableH->value();
            const double tableD = widget_.spinBoxTableD->value();
            const double margin = widget_.spinBoxMarginBoxWalls->value();
 
            const double minX = tableX - tableW / 2 + margin;
            const double minY = tableY - tableH / 2 + margin;
            const double maxX = tableX + tableW / 2 - margin;
            const double maxY = tableY + tableH / 2 - margin;
            const double minZ = tableZ + tableD + 150;
 
            widget_.spinBoxObjectMinX->setValue(minX);
            widget_.spinBoxObjectMinY->setValue(minY);
            widget_.spinBoxObjectMaxX->setValue(maxX);
            widget_.spinBoxObjectMaxY->setValue(maxY);
            widget_.spinBoxObjectMinZ->setValue(minZ);
        }
    }
 
    void
    ClutteredSceneGeneratorWidgetController::generateScene()
    {
        simulation::scene_generation::ClutteredSceneGenerator generator(simulator_, getGenConfig());
        int seed = widget_.spinBoxSceneSeed->value();
        generator.generateScene(seed);
    }
 
    simulation::scene_generation::ClutteredSceneGenerator::Config
    ClutteredSceneGeneratorWidgetController::getGenConfig()
    {
        simulation::scene_generation::ClutteredSceneGenerator::Config genConfig;
 
        // Set selected object sets
        std::vector<simulation::scene_generation::ObjectSet> selectedSets;
 
        for (int i = 0; i < widget_.listWidgetObjectSets->count(); ++i)
        {
            auto* item = widget_.listWidgetObjectSets->item(i);
            if (item->checkState() == Qt::CheckState::Checked)
            {
                std::string setPath = item->data(1).toString().toStdString();
                selectedSets.push_back(objectSets_[setPath]);
            }
        }
        genConfig.objectSets = selectedSets;
 
        genConfig.amountObjects = widget_.spinBoxSceneObjectAmount->value();
        // genConfig.objectsPerSetLimit = widget_.checkBoxLimitOnePerSet->isChecked() ? 1 : 0;
        genConfig.fallingSteps = widget_.spinBoxObjectFallingSteps->value();
        genConfig.autoResetInvalidObjects = widget_.checkBoxResetInvalidObjects->isChecked();
        genConfig.maxResetTries = widget_.spinBoxMaxResetAttempts->value();
 
        genConfig.boxX = static_cast<float>(widget_.spinBoxTableX->value());
        genConfig.boxY = static_cast<float>(widget_.spinBoxTableY->value());
        genConfig.boxZ = static_cast<float>(widget_.spinBoxTableZ->value());
        genConfig.boxW = static_cast<float>(widget_.spinBoxTableW->value());
        genConfig.boxH = static_cast<float>(widget_.spinBoxTableH->value());
        genConfig.boxD = static_cast<float>(widget_.spinBoxTableD->value());
 
        genConfig.objectMarginSide = static_cast<float>(widget_.spinBoxMarginBoxWalls->value());
        genConfig.objectSpacingZ = static_cast<float>(widget_.spinBoxObjectSpacingZ->value());
 
        genConfig.minObjectX = static_cast<float>(widget_.spinBoxObjectMinX->value());
        genConfig.minObjectY = static_cast<float>(widget_.spinBoxObjectMinY->value());
        genConfig.minObjectZ = static_cast<float>(widget_.spinBoxObjectMinZ->value());
        genConfig.maxObjectX = static_cast<float>(widget_.spinBoxObjectMaxX->value());
        genConfig.maxObjectY = static_cast<float>(widget_.spinBoxObjectMaxY->value());
 
        genConfig.minObjectZOtherwiseReset =
            static_cast<float>(widget_.spinBoxZPosResetting->value());
 
        return genConfig;
    }
} // namespace armarx