Visualizer.h

Go to the documentation of this file.

#pragma once
 
#include <functional>
#include <memory>
#include <mutex>
#include <string>
#include <typeindex>
 
#include <IceUtil/Shared.h>
 
#include <RobotAPI/interface/ArViz/Component.h>
 
#include "ElementVisualizer.h"
#include <Inventor/manips/SoTransformerManip.h>
#include <Inventor/nodes/SoSelection.h>
#include <Inventor/nodes/SoSeparator.h>
 
namespace armarx::viz
{
    using CoinLayerID = std::pair<std::string, std::string>;
 
    struct CoinLayerElement
    {
        data::ElementPtr data;
        std::unique_ptr<coin::ElementVisualization> visu;
    };
 
    inline bool
    isElementIdLess(CoinLayerElement const& left, CoinLayerElement const& right)
    {
        return left.data->id < right.data->id;
    }
 
    struct CoinLayer
    {
        CoinLayer() = default;
 
        CoinLayer(CoinLayerID const& id, SoSeparator* node) : id(id), node(node)
        {
            // Increase ref-count, so we can add/remove this node without it being deleted
            // This is needed for showing/hiding layers
            node->ref();
            pivot.data = new data::Element;
        }
 
        CoinLayer(CoinLayer&& other) :
            id(std::move(other.id)),
            node(other.node),
            elements(std::move(other.elements)),
            pivot(std::move(other.pivot))
        {
            other.node = nullptr;
        }
 
        ~CoinLayer()
        {
            if (node)
            {
                node->unref();
            }
        }
 
        CoinLayer(CoinLayer const&) = delete;
        void operator=(CoinLayer const&) = delete;
 
        void
        operator=(CoinLayer&& other)
        {
            id = std::move(other.id);
            node = other.node;
            other.node = nullptr;
            elements = std::move(other.elements);
        }
 
        auto
        lowerBound(std::string const& id)
        {
            pivot.data->id = id;
            return std::lower_bound(elements.begin(), elements.end(), pivot, &isElementIdLess);
        }
 
        CoinLayerElement*
        findElement(std::string const& id)
        {
            auto iter = lowerBound(id);
            if (iter != elements.end() && iter->data->id == id)
            {
                return &*iter;
            }
            return nullptr;
        }
 
        CoinLayerElement const*
        findElement(std::string const& id) const
        {
            return const_cast<CoinLayer*>(this)->findElement(id);
        }
 
        CoinLayerID id;
        SoSeparator* node = nullptr;
        std::vector<CoinLayerElement> elements;
        CoinLayerElement pivot;
    };
 
    inline bool
    isCoinLayerIdLess(CoinLayer const& left, CoinLayer const& right)
    {
        return left.id < right.id;
    }
 
    struct CoinLayerMap
    {
        auto
        lowerBound(CoinLayerID const& id)
        {
            pivot.id = id;
            return std::lower_bound(data.begin(), data.end(), pivot, &isCoinLayerIdLess);
        }
 
        CoinLayer*
        findLayer(CoinLayerID const& id)
        {
            auto iter = lowerBound(id);
            if (iter != data.end() && iter->id == id)
            {
                return &*iter;
            }
            return nullptr;
        }
 
        CoinLayer const*
        findLayer(CoinLayerID const& id) const
        {
            return const_cast<CoinLayerMap*>(this)->findLayer(id);
        }
 
        std::vector<CoinLayer> data;
        CoinLayer pivot;
    };
 
    enum class CoinVisualizerState
    {
        STOPPED,
        STARTING,
        RUNNING,
        STOPPING
    };
 
    enum class CoinVisualizerUpdateResult
    {
        SUCCESS,
        WAITING,
        FAILURE,
    };
 
    struct CoinVisualizer_ApplyTiming
    {
        std::string layerName;
 
        IceUtil::Time addLayer = IceUtil::Time::seconds(0);
        IceUtil::Time updateElements = IceUtil::Time::seconds(0);
        IceUtil::Time removeElements = IceUtil::Time::seconds(0);
        IceUtil::Time total = IceUtil::Time::seconds(0);
 
        void
        add(CoinVisualizer_ApplyTiming const& other)
        {
            addLayer += other.addLayer;
            updateElements += other.updateElements;
            removeElements += other.removeElements;
            total += other.total;
        }
    };
 
    struct CoinVisualizer_UpdateTiming
    {
        std::vector<CoinVisualizer_ApplyTiming> applies;
 
        CoinVisualizer_ApplyTiming applyTotal;
 
        IceUtil::Time pull = IceUtil::Time::seconds(0);
        IceUtil::Time layersChanged = IceUtil::Time::seconds(0);
        IceUtil::Time total = IceUtil::Time::seconds(0);
 
        IceUtil::Time waitStart = IceUtil::Time::seconds(0);
        IceUtil::Time waitDuration = IceUtil::Time::seconds(0);
 
        int updateToggle = 0;
 
        int counter = 0;
    };
 
    struct CoinVisualizerWrapper;
 
    struct ElementInteractionData
    {
        CoinLayerID layer;
        std::string element;
        viz::data::InteractionDescription interaction;
        coin::ElementVisualization* visu = nullptr;
    };
 
    class CoinVisualizer
    {
    public:
        CoinVisualizer();
 
        ~CoinVisualizer();
 
        void clearCache();
 
        void registerVisualizationTypes();
 
        void startAsync(StorageInterfacePrx const& storage);
 
        void stop();
 
        CoinVisualizer_ApplyTiming apply(data::LayerUpdate const& update);
 
        void update();
 
        void exportToVRML(std::string const& exportFilePath);
 
        template <typename ElementVisuT>
        void
        registerVisualizerFor()
        {
            using ElementT = typename ElementVisuT::ElementType;
            using VisualizerT = coin::TypedElementVisualizer<ElementVisuT>;
            // Map element type to visualizer
            elementVisualizersTypes.emplace_back(typeid(ElementT));
            elementVisualizers.push_back(std::make_unique<VisualizerT>());
        }
 
        void showLayer(CoinLayerID const& id, bool visible);
 
        CoinVisualizer_UpdateTiming getTiming();
 
        CoinLayer& findOrAddLayer(CoinLayerID const& layerID);
        void addOrUpdateElements(CoinLayer* layer, data::LayerUpdate const& update);
        void addOrUpdateInteraction(CoinLayerID const& layerID,
                                    std::string const& elementID,
                                    data::InteractionDescription const& interactionDesc,
                                    coin::ElementVisualization* visu);
        void removeElementsIfNotUpdated(CoinLayer* layer);
 
        std::vector<CoinLayerID> getLayerIDs();
        void emitLayersChanged(std::vector<CoinLayerID> const& layerIDs);
        void emitLayerUpdated(CoinLayerID const& layerID, CoinLayer const& layer);
 
        void selectElement(int index);
        void onSelectEvent(SoPath* path, int eventType);
        void onManipulation(SoDragger* dragger, int eventType);
        // These are selectable element IDs and need to be persistent in memory.
        // We store a raw pointer to these into the SoSeperator objects of Coin.
        // Later, we can retrieve the element ID from this pointer, if it has been selected.
        std::vector<std::unique_ptr<ElementInteractionData>> elementInteractions;
        std::vector<viz::data::InteractionFeedback> interactionFeedbackBuffer;
        // The currently selected element. Maybe nullptr if no element is selected.
        ElementInteractionData* selectedElement = nullptr;
 
        void onUpdateSuccess(data::LayerUpdates const& updates);
        void onUpdateFailure(Ice::Exception const& ex);
        IceUtil::Handle<CoinVisualizerWrapper> callbackData;
        armarx::viz::Callback_StorageInterface_pullUpdatesSinceAndSendInteractionsPtr callback;
 
        std::mutex storageMutex;
        viz::StorageInterfacePrx storage;
 
        CoinLayerMap layers;
 
        std::vector<std::type_index> elementVisualizersTypes;
        std::vector<std::unique_ptr<coin::ElementVisualizer>> elementVisualizers;
 
        SoSelection* selection = nullptr;
        SoSeparator* root = nullptr;
        SoSeparator* manipulatorGroup = nullptr;
        SoTransformerManip* manipulator = nullptr;
 
        std::atomic<CoinVisualizerUpdateResult> updateResult{CoinVisualizerUpdateResult::SUCCESS};
        data::LayerUpdates pulledUpdates;
 
        std::mutex stateMutex;
        std::atomic<CoinVisualizerState> state{CoinVisualizerState::STOPPED};
        viz::StorageInterfacePrx stateStorage;
 
        std::function<void(std::vector<CoinLayerID> const&)> layersChangedCallback;
 
        /// A layer's data has changed.
        std::vector<std::function<void(CoinLayerID const& layerID, CoinLayer const& layer)>>
            layerUpdatedCallbacks;
 
        std::mutex timingMutex;
        CoinVisualizer_UpdateTiming lastTiming;
    };
} // namespace armarx::viz