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 <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/DecoupledMain.h>
 
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.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);
} // namespace armarx
 
int
main(int argc, char* argv[])
{
    return armarx::DecoupledMain(argc, argv);
}