Component.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::ArmarXObjects::cluttered_scene_generator
 * @author     Patrick Hegemann ( 335495-patrickhegemann at users dot noreply dot gitlab dot com )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "Component.h"
 
#include <ArmarXSimulation/libraries/ClutteredSceneGenerator/ClutteredSceneGenerator.h>
#include <ArmarXSimulation/libraries/ClutteredSceneGenerator/objects/SimulatedObject.h>
#include <ArmarXSimulation/interface/simulator/SimulatorInterface.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/ArmarXObjects/Scene.h>
#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <SimoxUtility/json/io.h>
#include <SimoxUtility/math/pose/pose.h>
 
#include <Eigen/src/Core/Matrix.h>
#include <Eigen/src/Geometry/Quaternion.h>
 
#include <algorithm>
#include <iterator>
 
namespace armarx::simulation::components::cluttered_scene_generator
{
    const std::string
    Component::defaultName = "ClutteredSceneGenerator";
 
    armarx::PropertyDefinitionsPtr
    Component::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def = new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
        // Use (and depend on) another component (passing the ComponentInterfacePrx).
        def->component(simulator_);
        def->component(objectMemoryToSimulation_);
 
        // Add a required property. (The component won't start without a value being set.)
        // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
 
        def->optional(properties_.out.packageName, "p.out.packageName");
        def->optional(properties_.out.packagePath, "p.out.packagePath");
 
        def->optional(properties_.gen.initialSeed, "p.initialSeed", "Initial random seed for scene generation");
 
        def->optional(properties_.gen.nScenes, "p.gen.nScenes", "Number of scenes to generate");
        def->optional(properties_.gen.nObjects, "p.gen.nObjects", "Number of objects to generate");
        def->optional(properties_.gen.verticalSpacing, "p.gen.verticalSpacing", "Vertical spacing between objects as they are generated");
        // def->op
 
        return def;
    }
 
    void
    Component::onInitComponent()
    {
 
    }
 
    void
    Component::onConnectComponent()
    {
        ARMARX_TRACE;
        // Do things after connecting to topics and components.
 
        /* (Requires the armarx::ArVizComponentPluginUser.)
        // Draw boxes in ArViz.
        // (Before starting any threads, we don't need to lock mutexes.)
        drawBoxes(properties, arviz);
        */
 
        /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
        // Setup the remote GUI.
        {
            createRemoteGuiTab();
            RemoteGui_startRunningTask();
        }
        */
 
        scene_generation::ObjectSet objectClasses = getObjectClasses();
 
        // TODO(patrick.hegemann): Refactor
        const scene_generation::ClutteredSceneGenerator::Config config
        {
            .objectSets = {objectClasses},
            .amountObjects = properties_.gen.nObjects,
            .objectSpacingZ = properties_.gen.verticalSpacing,
            .minObjectX = -200,
            .minObjectY = 800,
            .maxObjectX = 200,
            .maxObjectY = 1200,
            .minObjectZ = 1000,
        };
 
        for (uint i = 0; i < properties_.gen.nScenes; ++i)
        {
            simulator_->reInitialize();
            objectMemoryToSimulation_->synchronizeSimulator();
 
            int seed = properties_.gen.initialSeed + static_cast<int>(i);
            generateScene(config, seed);
 
            objects::Scene scene = getScene();
 
            if (!properties_.out.packageName.empty() && !properties_.out.packagePath.empty())
            {
                armarx::PackagePath path(properties_.out.packageName, properties_.out.packagePath);
                std::filesystem::path sceneFileName = path.toSystemPath() / ("scene_" + std::to_string(i) + ".json");
                storeScene(scene, sceneFileName);
            }
        }
    }
 
    void
    Component::onDisconnectComponent()
    {
    }
 
    void
    Component::onExitComponent()
    {
    }
 
    std::string
    Component::getDefaultName() const
    {
        return Component::defaultName;
    }
 
    std::string
    Component::GetDefaultName()
    {
        return Component::defaultName;
    }
 
    inline armarx::PackagePath toPackagePath(const armarx::PackageFileLocation& packageFileLocation)
    {
        return {packageFileLocation.package, packageFileLocation.relativePath};
    }
 
    scene_generation::ObjectSet Component::getObjectClasses()
    {
        ARMARX_TRACE;
        scene_generation::ObjectSet objectSet;
 
        ObjectFinder f;
        std::vector<ObjectInfo> objectInfos = f.findAllObjectsOfDataset("KIT");
 
        auto makeObjectSource = [&](const ObjectInfo& info) {
            return scene_generation::ObjectSource {
                .path = toPackagePath(info.simoxXML()),
                .type = scene_generation::SimulatedObjectType::AsObject,
                .objectID = info.id()
            };
        };
 
        std::transform(objectInfos.begin(), objectInfos.end(),
                       std::back_inserter(objectSet.objects),
                       makeObjectSource);
 
        return objectSet;
    }
 
    void Component::generateScene(const scene_generation::ClutteredSceneGenerator::Config& config, const int seed)
    {
        ARMARX_TRACE;
        scene_generation::ClutteredSceneGenerator gen(simulator_, config);
        gen.generateScene(seed);
    }
 
    objects::Scene Component::getScene()
    {
        ARMARX_TRACE;
 
        objects::Scene scene;
        armarx::SceneVisuData sceneData = simulator_->getScene();
 
        auto makeSceneObject = [](const armarx::ObjectVisuData& simObject) {
            const armarx::ObjectID objectID(simObject.name);
            // const armarx::ObjectID objectID(simObject.project, simObject.objectClassName, simObject.name);
            Eigen::Matrix4f objectPose {armarx::fromIce(simObject.objectPoses.at(simObject.name))};
 
            return objects::SceneObject {
                .className = objectID.getClassID().str(),
                .instanceName = objectID.instanceName(),
                .collection = objectID.dataset(),
                .position = simox::math::position(objectPose),
                .orientation = Eigen::Quaternionf(simox::math::orientation(objectPose)),
                .isStatic = false,
                .jointValues = {}
            };
        };
 
        std::transform(sceneData.objects.begin(), sceneData.objects.end(),
                       std::back_inserter(scene.objects),
                       makeSceneObject);
 
        return scene;
    }
 
    void Component::storeScene(const objects::Scene& scene, const std::filesystem::path& path)
    {
        ARMARX_TRACE;
 
        ARMARX_INFO << "Storing scene snapshot at: \n\t" << path;
        try
        {
            simox::json::write(path, scene, 2);
        }
        catch (const simox::json::error::JsonError& e)
        {
            ARMARX_WARNING << "Storing scene snapshot failed: \n" << e.what();
        }
    }
 
    /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
    void
    Component::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        // Setup the widgets.
 
        tab.boxLayerName.setValue(properties.boxLayerName);
 
        tab.numBoxes.setValue(properties.numBoxes);
        tab.numBoxes.setRange(0, 100);
 
        tab.drawBoxes.setLabel("Draw Boxes");
 
        // Setup the layout.
 
        GridLayout grid;
        int row = 0;
        {
            grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
            ++row;
 
            grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
            ++row;
 
            grid.add(tab.drawBoxes, {row, 0}, {2, 1});
            ++row;
        }
 
        VBoxLayout root = {grid, VSpacer()};
        RemoteGui_createTab(getName(), root, &tab);
    }
 
 
    void
    Component::RemoteGui_update()
    {
        if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
        {
            std::scoped_lock lock(propertiesMutex);
            properties.boxLayerName = tab.boxLayerName.getValue();
            properties.numBoxes = tab.numBoxes.getValue();
 
            {
                setDebugObserverDatafield("numBoxes", properties.numBoxes);
                setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
                sendDebugObserverBatch();
            }
        }
        if (tab.drawBoxes.wasClicked())
        {
            // Lock shared variables in methods running in seperate threads
            // and pass them to functions. This way, the called functions do
            // not need to think about locking.
            std::scoped_lock lock(propertiesMutex, arvizMutex);
            drawBoxes(properties, arviz);
        }
    }
    */
 
 
    /* (Requires the armarx::ArVizComponentPluginUser.)
    void
    Component::drawBoxes(const Component::Properties& p, viz::Client& arviz)
    {
        // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
        // See the ArVizExample in RobotAPI for more examples.
 
        viz::Layer layer = arviz.layer(p.boxLayerName);
        for (int i = 0; i < p.numBoxes; ++i)
        {
            layer.add(viz::Box("box_" + std::to_string(i))
                      .position(Eigen::Vector3f(i * 100, 0, 0))
                      .size(20).color(simox::Color::blue()));
        }
        arviz.commit(layer);
    }
    */
 
    // TODO(patrick.hegemann): enable after migration:
    // ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
}  // namespace armarx::simulation::components::cluttered_scene_generator