ArVizInteractExample.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    RobotAPI::ArmarXObjects::ArVizInteractExample
 * @author     Fabian Paus ( fabian dot paus at kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/DecoupledMain.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/Component.h>
 
namespace armarx
{
 
    struct SingleSlider
    {
        SingleSlider(std::string const& name, viz::Color color)
            : box(name)
            , color(color)
        {
 
        }
 
        viz::Box box;
        viz::Color color;
 
        Eigen::Vector3f initial = Eigen::Vector3f::Zero();
        Eigen::Vector3f translation = Eigen::Vector3f::Zero();
 
    };
 
    struct SlidersState
    {
        SlidersState(Eigen::Vector3f origin)
            : origin(origin)
            , x("BoxX", viz::Color::red())
            , y("BoxY", viz::Color::green())
            , z("BoxZ", viz::Color::blue())
            , sphere("Sphere")
        {
            float boxSize = 50.0f;
 
            x.initial = origin + Eigen::Vector3f(0.5f * ARROW_LENGTH, 0.0f, 0.0f);
            x.box.position(x.initial)
                 .color(x.color)
                 .size(boxSize)
                 .enable(viz::interaction()
                         .translation(viz::AXES_X)
                         .hideDuringTransform());
 
            y.initial = origin + Eigen::Vector3f(0.0f, 0.5f * ARROW_LENGTH, 0.0f);
            y.box.position(y.initial)
                 .color(y.color)
                 .size(boxSize)
                 .enable(viz::interaction()
                         .translation(viz::AXES_Y)
                         .hideDuringTransform());
 
            z.initial = origin + Eigen::Vector3f(0.0f, 0.0f, 0.5f * ARROW_LENGTH);
            z.box.position(z.initial)
                 .color(z.color)
                 .size(boxSize)
                 .enable(viz::interaction()
                         .translation(viz::AXES_Z)
                         .hideDuringTransform());
 
            sphere.position(origin + 0.5f * ARROW_LENGTH * Eigen::Vector3f(1.0f, 1.0f, 1.0f))
                  .color(viz::Color::orange())
                  .radius(30.0f);
        }
 
        static constexpr const float ARROW_LENGTH = 1000.0f;
 
        void visualize(viz::Client& arviz)
        {
            layerInteract = arviz.layer("Sliders");
 
            float arrowWidth = 10.0f;
 
            viz::Arrow arrowX = viz::Arrow("ArrowX")
                                .color(viz::Color::red())
                                .fromTo(origin, origin + Eigen::Vector3f(ARROW_LENGTH, 0.0f, 0.0f))
                                .width(arrowWidth);
            layerInteract.add(arrowX);
 
            viz::Arrow arrowY = viz::Arrow("ArrowY")
                                .color(viz::Color::green())
                                .fromTo(origin, origin + Eigen::Vector3f(0.0f, ARROW_LENGTH, 0.0f))
                                .width(arrowWidth);
            layerInteract.add(arrowY);
 
            viz::Arrow arrowZ = viz::Arrow("ArrowZ")
                                .color(viz::Color::blue())
                                .fromTo(origin, origin + Eigen::Vector3f(0.0f, 0.0f, ARROW_LENGTH))
                                .width(arrowWidth);
            layerInteract.add(arrowZ);
 
 
            layerInteract.add(x.box);
            layerInteract.add(y.box);
            layerInteract.add(z.box);
 
            layerResult = arviz.layer("SlidersResult");
            layerResult.add(sphere);
        }
 
        void handle(viz::InteractionFeedback const& interaction,
                    viz::StagedCommit* stage)
        {
            std::string const& element = interaction.element();
            Eigen::Matrix4f transform = interaction.transformation();
            Eigen::Vector3f translation = transform.block<3, 1>(0, 3);
 
            SingleSlider* slider = nullptr;
            if (element == "BoxX")
            {
                slider = &x;
            }
            else if (element == "BoxY")
            {
                slider = &y;
            }
            else if (element == "BoxZ")
            {
                slider = &z;
            }
            else
            {
                ARMARX_WARNING << "Unknown interaction: " << element;
                return;
            }
 
            switch (interaction.type())
            {
            case viz::InteractionFeedbackType::Transform:
            {
                slider->translation = translation;
 
                Eigen::Vector3f spherePosition(
                            x.initial.x() + x.translation.x(),
                            y.initial.y() + y.translation.y(),
                            z.initial.z() + z.translation.z());
                sphere.position(spherePosition);
 
                stage->add(layerResult);
            } break;
 
            case viz::InteractionFeedbackType::Select:
            {
                // Do nothing
            } break;
 
            case viz::InteractionFeedbackType::Deselect:
            {
                // If an object is deselected, we apply the transformation
                slider->initial = slider->initial + slider->translation;
                slider->translation = Eigen::Vector3f::Zero();
                ARMARX_IMPORTANT << "Setting position to "
                                 << slider->initial.transpose();
                slider->box.position(slider->initial);
 
                stage->add(layerInteract);
            } break;
 
            default:
            {
                // Do nothing for the other interaction types
            } break;
            }
 
 
        }
 
        Eigen::Vector3f origin;
        SingleSlider x;
        SingleSlider y;
        SingleSlider z;
 
        viz::Sphere sphere;
 
        viz::Layer layerInteract;
        viz::Layer layerResult;
    };
 
 
    // ---------------
 
    // What abstractions are needed?
    // MovableElement
    // SpawnerElement
 
 
 
 
    struct SlidersState2
    {
        SlidersState2(Eigen::Vector3f origin)
            : origin(origin)
            , x("BoxX")
            , y("BoxY")
            , z("BoxZ")
            , sphere("Sphere")
        {
            float boxSize = 50.0f;
 
            // We use the Transformable<T>::position to set the position
            // This keeps track of the internal state
            x.position(origin + Eigen::Vector3f(0.5f * ARROW_LENGTH, 0.0f, 0.0f));
            // A movable object is always hidden during the transformation
            // The hideDuringTransform() flag is automatically set here
            x.enable(viz::interaction().translation(viz::AXES_X));
 
            // Other attributes of the element can be set directly on the member
            x.element.color(viz::Color::red())
                    .size(boxSize);
 
            y.position(origin + Eigen::Vector3f(0.0f, 0.5f * ARROW_LENGTH, 0.0f));
            y.enable(viz::interaction().translation(viz::AXES_Y));
 
            y.element.color(viz::Color::green())
                    .size(boxSize);
 
            z.position(origin + Eigen::Vector3f(0.0f, 0.0f, 0.5f * ARROW_LENGTH));
            z.enable(viz::interaction().translation(viz::AXES_Z));
            z.element.color(viz::Color::blue())
                    .size(boxSize);
 
            sphere.position(origin + 0.5f * ARROW_LENGTH * Eigen::Vector3f(1.0f, 1.0f, 1.0f))
                  .color(viz::Color::orange())
                  .radius(30.0f);
        }
 
        static constexpr const float ARROW_LENGTH = 1000.0f;
 
        void visualize(viz::Client& arviz)
        {
            layerInteract = arviz.layer("Sliders2");
 
            float arrowWidth = 10.0f;
 
            viz::Arrow arrowX = viz::Arrow("ArrowX")
                                .color(viz::Color::red())
                                .fromTo(origin, origin + Eigen::Vector3f(ARROW_LENGTH, 0.0f, 0.0f))
                                .width(arrowWidth);
            layerInteract.add(arrowX);
 
            viz::Arrow arrowY = viz::Arrow("ArrowY")
                                .color(viz::Color::green())
                                .fromTo(origin, origin + Eigen::Vector3f(0.0f, ARROW_LENGTH, 0.0f))
                                .width(arrowWidth);
            layerInteract.add(arrowY);
 
            viz::Arrow arrowZ = viz::Arrow("ArrowZ")
                                .color(viz::Color::blue())
                                .fromTo(origin, origin + Eigen::Vector3f(0.0f, 0.0f, ARROW_LENGTH))
                                .width(arrowWidth);
            layerInteract.add(arrowZ);
 
 
            layerInteract.add(x.element);
            layerInteract.add(y.element);
            layerInteract.add(z.element);
 
            layerResult = arviz.layer("SlidersResult2");
            layerResult.add(sphere);
        }
 
        void handle(viz::InteractionFeedback const& interaction,
                    viz::StagedCommit* stage)
        {
            // Let the Transformable<T> handle all events internally
            bool needsLayerUpdate = false;
            viz::TransformationResult xResult = x.handle(interaction);
            needsLayerUpdate |= xResult.needsLayerUpdate;
            needsLayerUpdate |= y.handle(interaction).needsLayerUpdate;
            needsLayerUpdate |= z.handle(interaction).needsLayerUpdate;
            if (needsLayerUpdate)
            {
                // At least one Transformable<T> indicated that the layer needs to be updated
                stage->add(layerInteract);
            }
 
            // We could react to specific events
            if (xResult.wasTransformed)
            {
                ARMARX_INFO << "The x slider was transformed: " << x.getCurrentPose().col(3).x();
            }
 
            // We handle the transform event ourselves to add custom behavior
            // Here, we move the sphere based on the position of the sliders
            if (interaction.type() == viz::InteractionFeedbackType::Transform)
            {
                // We can query getCurrentPose() to determine the poses of the sliders
                // with the transformation applied to them
                Eigen::Vector3f spherePosition(
                            x.getCurrentPose().col(3).x(),
                            y.getCurrentPose().col(3).y(),
                            z.getCurrentPose().col(3).z());
                sphere.position(spherePosition);
 
                stage->add(layerResult);
            }
        }
 
        Eigen::Vector3f origin;
        viz::Transformable<viz::Box> x;
        viz::Transformable<viz::Box> y;
        viz::Transformable<viz::Box> z;
 
        viz::Sphere sphere;
 
        viz::Layer layerInteract;
        viz::Layer layerResult;
    };
    // ---------------
 
 
    enum class SpawnerType
    {
        Box,
        Cylinder,
        Sphere,
    };
 
    enum class SpawnerOption
    {
        DeleteAll = 0,
        DeleteType = 1,
    };
 
    struct Spawner
    {
        SpawnerType type = SpawnerType::Box;
 
        Eigen::Vector3f position = Eigen::Vector3f::Zero();
        float size = 100.0f;
        viz::Color color = viz::Color::black();
 
        void visualize(int i, viz::Layer& layer)
        {
            viz::InteractionDescription interaction = viz::interaction()
                                                      .selection().transform().scaling()
                                                      .contextMenu({"Delete All", "Delete All of Type"});
            std::string name = "Spawner_" + std::to_string(i);
            switch (type)
            {
            case SpawnerType::Box:
            {
                viz::Box box = viz::Box(name)
                               .position(position)
                               .size(size)
                               .color(color)
                               .enable(interaction);
                layer.add(box);
            } break;
            case SpawnerType::Cylinder:
            {
                viz::Cylinder cylinder = viz::Cylinder(name)
                                         .position(position)
                                         .radius(size*0.5f)
                                         .height(size)
                                         .color(color)
                                         .enable(interaction);
                layer.add(cylinder);
            } break;
            case SpawnerType::Sphere:
            {
                viz::Sphere sphere = viz::Sphere(name)
                                     .position(position)
                                     .radius(size*0.5f)
                                     .color(color)
                                     .enable(interaction);
                layer.add(sphere);
            } break;
            }
        }
    };
 
    struct SpawnedObject
    {
        int index = 0;
        Spawner* source = nullptr;
        Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
        Eigen::Vector3f scale = Eigen::Vector3f::Ones();
 
        void visualize(viz::Layer& layer)
        {
            if (source == nullptr)
            {
                ARMARX_WARNING << "Tried to visualize a spawned object that does not have a source";
                return;
            }
 
            viz::InteractionDescription interaction = viz::interaction()
                                                      .selection()
                                                      .contextMenu({"Delete"});
            std::string name = "Object_" + std::to_string(index);
 
            Eigen::Matrix4f initial = Eigen::Matrix4f::Identity();
            initial.block<3, 1>(0, 3) = source->position;
            Eigen::Matrix4f pose = transform * initial;
 
            switch (source->type)
            {
            case SpawnerType::Box:
            {
                viz::Box box = viz::Box(name)
                               .pose(pose)
                               .scale(scale)
                               .size(source->size)
                               .color(source->color)
                               .enable(interaction);
                layer.add(box);
            } break;
            case SpawnerType::Cylinder:
            {
                viz::Cylinder cylinder = viz::Cylinder(name)
                                         .pose(pose)
                                         .scale(scale)
                                         .radius(source->size * 0.5f)
                                         .height(source->size)
                                         .color(source->color)
                                         .enable(interaction);
                layer.add(cylinder);
            } break;
            case SpawnerType::Sphere:
            {
                viz::Sphere sphere = viz::Sphere(name)
                                     .pose(pose)
                                     .scale(scale)
                                     .radius(source->size * 0.5f)
                                     .color(source->color)
                                     .enable(interaction);
                layer.add(sphere);
            } break;
            }
        }
    };
 
    struct SpawnersState
    {
        SpawnersState(Eigen::Vector3f origin)
            : origin(origin)
        {
            float size = 100.0f;
            {
                Spawner& spawner = spawners.emplace_back();
                spawner.type = SpawnerType::Box;
                spawner.position = origin + Eigen::Vector3f(750.0f, 500.0f, 0.5f * size);
                spawner.color = viz::Color::cyan();
            }
            {
                Spawner& spawner = spawners.emplace_back();
                spawner.type = SpawnerType::Cylinder;
                spawner.position = origin + Eigen::Vector3f(1250.0f, 500.0f, 0.5f * size);
                spawner.color = viz::Color::magenta();
            }
            {
                Spawner& spawner = spawners.emplace_back();
                spawner.type = SpawnerType::Sphere;
                spawner.position = origin + Eigen::Vector3f(1000.0f, 750.0f, 0.5f * size);
                spawner.color = viz::Color::yellow();
            }
        }
 
        void visualize(viz::Client& arviz)
        {
            layerSpawners = arviz.layer("Spawners");
 
            int index = 0;
            for (Spawner& spawner: spawners)
            {
                spawner.visualize(index, layerSpawners);
                index += 1;
            }
 
            layerObjects = arviz.layer("SpawnedObjects");
        }
 
        void visualizeSpawnedObjects(viz::StagedCommit* stage)
        {
            layerObjects.clear();
            for (auto& object : objects)
            {
                object.visualize(layerObjects);
            }
            stage->add(layerObjects);
        }
 
        void handle(viz::InteractionFeedback const& interaction,
                    viz::StagedCommit* stage)
        {
            Spawner* spawner = nullptr;
            for (int i = 0; i < (int)spawners.size(); ++i)
            {
                std::string name = "Spawner_" + std::to_string(i);
                if (interaction.element() == name)
                {
                    spawner = &spawners[i];
                    break;
                }
            }
            if (spawner == nullptr)
            {
                ARMARX_INFO << "No spawner" << interaction.element();
                // A spawned object was selected instead of a spawner
                if (interaction.type() == viz::InteractionFeedbackType::ContextMenuChosen
                        && interaction.chosenContextMenuEntry() == 0)
                {
                    // The delete context menu option was chosen
                    // So we remove the object from the internal list and redraw
                    auto newEnd = std::remove_if(objects.begin(), objects.end(),
                                   [&interaction](SpawnedObject const& object)
                    {
                        std::string name = "Object_" + std::to_string(object.index);
                        return interaction.element() == name;
                    });
                    objects.erase(newEnd, objects.end());
 
                    visualizeSpawnedObjects(stage);
                }
                return;
            }
 
            switch (interaction.type())
            {
            case viz::InteractionFeedbackType::Select:
            {
                // Create a spawned object
                spawnedObject.index = spawnedObjectCounter++;
                spawnedObject.source = spawner;
                spawnedObject.transform = Eigen::Matrix4f::Identity();
                spawnedObject.scale.setOnes();
            } break;
 
            case viz::InteractionFeedbackType::Transform:
            {
                // Update state of spawned object
                spawnedObject.transform = interaction.transformation();
                spawnedObject.scale = interaction.scale();
                if (interaction.isTransformBegin())
                {
                    // Visualize all other objects except the currently spawned one
                    visualizeSpawnedObjects(stage);
                }
                if (interaction.isTransformEnd())
                {
                    spawnedObject.visualize(layerObjects);
                    stage->add(layerObjects);
                }
            } break;
 
            case viz::InteractionFeedbackType::Deselect:
            {
                // Save state of spawned object
                objects.push_back(spawnedObject);
            } break;
 
            case viz::InteractionFeedbackType::ContextMenuChosen:
            {
                SpawnerOption option = (SpawnerOption)(interaction.chosenContextMenuEntry());
                switch (option)
                {
                case SpawnerOption::DeleteAll:
                {
                    objects.clear();
                    layerObjects.clear();
 
                    stage->add(layerObjects);
                } break;
                case SpawnerOption::DeleteType:
                {
                    auto newEnd = std::remove_if(objects.begin(), objects.end(),
                                   [spawner](SpawnedObject const& obj)
                    {
                       return obj.source == spawner;
                    });
                    objects.erase(newEnd, objects.end());
 
                    visualizeSpawnedObjects(stage);
                } break;
                }
            }
 
            default:
            {
                // Ignore other interaction types
            } break;
            }
        }
 
        Eigen::Vector3f origin;
 
        std::vector<Spawner> spawners;
        SpawnedObject spawnedObject;
        int spawnedObjectCounter = 0;
        std::vector<SpawnedObject> objects;
 
        viz::Layer layerSpawners;
        viz::Layer layerObjects;
    };
 
    /**
     * @defgroup Component-ArVizInteractExample ArVizInteractExample
     * @ingroup RobotAPI-Components
     *
     * An example for how to visualize 3D elements via the 3D visualization
     * framework ArViz.
     *
     * The example creates several layers, fills them with visualization
     * elements, and commits them to ArViz.
     *
     * To see the result:
     * \li Start the component `ArVizStorage`
     * \li Open the gui plugin `ArViz`
     * \li Start the component `ArVizInteractExample`
     *
     * The scenario `ArVizInteractExample` starts the necessary components,
     * including the example component.
     *
     *
     * A component which wants to visualize data via ArViz should:
     * \li `#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>`
     * \li Inherit from the `armarx::ArVizComponentPluginUser`. This adds the
     *     necessary properties (e.g. the topic name) and provides a
     *     ready-to-use ArViz client called `arviz`.
     * \li Use the inherited ArViz client variable `arviz` of type `viz::Client`
     *     to create layers, add visualization elements to the layers,
     *     and commit the layers to the ArViz topic.
     *
     * \see ArVizInteractExample
     *
     *
     * @class ArVizInteractExample
     * @ingroup Component-ArVizInteractExample
     *
     * @brief An example for how to use ArViz.
     *
     * @see @ref Component-ArVizInteractExample
     */
    struct ArVizInteractExample :
        virtual armarx::Component,
        // Deriving from armarx::ArVizComponentPluginUser adds necessary properties
        // and provides a ready-to-use ArViz client called `arviz`.
        virtual armarx::ArVizComponentPluginUser
    {
        std::string getDefaultName() const override
        {
            return "ArVizInteractExample";
        }
 
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
        {
            armarx::PropertyDefinitionsPtr defs(new ComponentPropertyDefinitions(getConfigIdentifier()));
            return defs;
        }
 
        void onInitComponent() override
        { }
 
        void onConnectComponent() override
        {
            task = new RunningTask<ArVizInteractExample>(this, &ArVizInteractExample::run);
            task->start();
        }
 
        void onDisconnectComponent() override
        {
            const bool join = true;
            task->stop(join);
            task = nullptr;
        }
 
        void onExitComponent() override
        { }
 
 
        void run()
        {
            viz::StagedCommit stage;
 
            viz::Layer regions = arviz.layer("Regions");
            Eigen::Vector3f origin1(-2000.0f, 0.0f, 0.0f);
            Eigen::Vector3f origin2(0.0f, 0.0f, 0.0f);
            Eigen::Vector3f origin3(-2000.0f, -2000.0f, 0.0f);
            Eigen::Vector3f origin4(0.0f, -2000.0f, 0.0f);
            {
                viz::Cylinder separatorX = viz::Cylinder("SeparatorX")
                                           .fromTo(origin1, origin1 + 4000.0f * Eigen::Vector3f::UnitX())
                                           .radius(5.0f);
                regions.add(separatorX);
 
                viz::Cylinder separatorY = viz::Cylinder("SeparatorY")
                                           .fromTo(origin4, origin4 + 4000.0f * Eigen::Vector3f::UnitY())
                                           .radius(5.0f);
                regions.add(separatorY);
 
                stage.add(regions);
            }
 
 
            SlidersState sliders(origin1 + Eigen::Vector3f(500.0f, 500.0f, 0.0f));
            SlidersState2 sliders2(origin3 + Eigen::Vector3f(500.0f, 500.0f, 0.0f));
            SpawnersState spawners(origin2);
 
            sliders.visualize(arviz);
            stage.add(sliders.layerInteract);
            stage.add(sliders.layerResult);
 
            sliders2.visualize(arviz);
            stage.add(sliders2.layerInteract);
            stage.add(sliders2.layerResult);
 
            spawners.visualize(arviz);
            stage.add(spawners.layerSpawners);
            stage.add(spawners.layerObjects);
 
            viz::CommitResult result = arviz.commit(stage);
            ARMARX_INFO << "Initial commit at revision: " << result.revision();
 
            CycleUtil c(10.0f);
            while (!task->isStopped())
            {
                result = arviz.commit(stage);
 
                // Reset the stage, so that it can be rebuild during the interaction handling
                stage.reset();
 
                stage.requestInteraction(sliders.layerInteract);
                stage.requestInteraction(sliders2.layerInteract);
                stage.requestInteraction(spawners.layerSpawners);
                stage.requestInteraction(spawners.layerObjects);
 
                viz::InteractionFeedbackRange interactions = result.interactions();
                for (viz::InteractionFeedback const& interaction : interactions)
                {
                    if (interaction.layer() == "Sliders")
                    {
                        sliders.handle(interaction, &stage);
                    }
                    if (interaction.layer() == "Sliders2")
                    {
                        sliders2.handle(interaction, &stage);
                    }
                    if (interaction.layer() == "Spawners" || interaction.layer() == "SpawnedObjects")
                    {
                        spawners.handle(interaction, &stage);
                    }
                }
 
                c.waitForCycleDuration();
            }
        }
 
        RunningTask<ArVizInteractExample>::pointer_type task;
 
    };
 
    ARMARX_DECOUPLED_REGISTER_COMPONENT(ArVizInteractExample);
}
 
int main(int argc, char* argv[])
{
    return armarx::DecoupledMain(argc, argv);
}