ArViz: Add selection callbacks

source/RobotAPI/components/ArViz/Client/elements/ElementOps.h

@@ -227,6 +227,7 @@ namespace armarx::viz
         DerivedT& enable(InteractionDescription const& interactionDescription)
         {
             data_->interaction = interactionDescription.data_;
+            return *static_cast<DerivedT*>(this);
         }
 
 

source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h

@@ -29,6 +29,7 @@ namespace armarx::viz::coin
         SoUnits* units;
         SoTransform* transform;
         SoMaterial* material;
+        // TODO: Transform to flag system
         bool wasUpdated = true;
         bool visible = true;
     };

source/RobotAPI/components/ArViz/Coin/Visualizer.cpp

@@ -34,6 +34,18 @@ namespace armarx::viz
         }
     };
 
+    static void selectionCallback(void* data, SoPath* path)
+    {
+        CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
+        this_->onSelection(path);
+    }
+
+    static void deselectionCallback(void* data, SoPath* path)
+    {
+        CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
+        this_->onDeselection(path);
+    }
+
     static const char* toString(CoinVisualizerState state)
     {
         switch (state)
@@ -86,7 +98,15 @@ namespace armarx::viz
         callback = newCallback_StorageInterface_pullUpdatesSince(callbackData,
                    &CoinVisualizerWrapper::onUpdateSuccess,
                    &CoinVisualizerWrapper::onUpdateFailure);
+
         root = new SoSeparator;
+
+        // The SoSelection node enable selection of nodes via mouse click / ray casting
+        selection = new SoSelection;
+        selection->addSelectionCallback(&selectionCallback, this);
+        selection->addDeselectionCallback(&deselectionCallback, this);
+
+        selection->addChild(root);
     }
 
     CoinVisualizer::~CoinVisualizer()
@@ -124,6 +144,24 @@ namespace armarx::viz
         state = CoinVisualizerState::STOPPED;
     }
 
+    static void removeElementsIfNotUpdated(CoinLayer* layer)
+    {
+        for (auto iter = layer->elements.begin(); iter != layer->elements.end();)
+        {
+            coin::ElementVisualization& elementVisu = *iter->visu;
+            if (elementVisu.wasUpdated)
+            {
+                elementVisu.wasUpdated = false;
+                ++iter;
+            }
+            else
+            {
+                layer->node->removeChild(elementVisu.separator);
+                iter = layer->elements.erase(iter);
+            }
+        }
+    }
+
     CoinVisualizer_ApplyTiming CoinVisualizer::apply(data::LayerUpdate const& update)
     {
         IceUtil::Time time_start = IceUtil::Time::now();
@@ -132,6 +170,31 @@ namespace armarx::viz
         timing.layerName = update.name;
 
         CoinLayerID layerID(update.component, update.name);
+        CoinLayer& layer = findOrAddLayer(layerID);
+
+        IceUtil::Time time_addLayer = IceUtil::Time::now();
+        timing.addLayer = time_addLayer - time_start;
+
+        addOrUpdateElements(&layer, update);
+
+        IceUtil::Time time_updates = IceUtil::Time::now();
+        timing.updateElements = time_updates - time_addLayer;
+
+        removeElementsIfNotUpdated(&layer);
+
+        IceUtil::Time time_remove = IceUtil::Time::now();
+        timing.removeElements = time_remove - time_updates;
+
+        emitLayerUpdated(layerID, layer);
+
+        IceUtil::Time time_end = IceUtil::Time::now();
+        timing.total = time_end - time_start;
+
+        return timing;
+    }
+
+    CoinLayer& CoinVisualizer::findOrAddLayer(CoinLayerID const& layerID)
+    {
         auto layerIt = layers.lowerBound(layerID);
 
         if (layerIt == layers.data.end() || layerIt->id != layerID)
@@ -144,13 +207,13 @@ namespace armarx::viz
             layerIt = layers.data.insert(layerIt, CoinLayer(layerID, coinNode));
         }
 
-        IceUtil::Time time_addLayer = IceUtil::Time::now();
-        timing.addLayer = time_addLayer - time_start;
+        return *layerIt;
+    }
 
-        // Add or update the elements in the update
-        CoinLayer& layer = *layerIt;
-        layer.elements.reserve(update.elements.size());
-        for (auto& updatedElementPtr : update.elements)
+    void CoinVisualizer::addOrUpdateElements(CoinLayer* layer, data::LayerUpdate const& update)
+    {
+        layer->elements.reserve(update.elements.size());
+        for (viz::data::ElementPtr const& updatedElementPtr : update.elements)
         {
             if (!updatedElementPtr)
             {
@@ -179,9 +242,9 @@ namespace armarx::viz
             }
             coin::ElementVisualizer* visualizer = elementVisualizers[visuIndex].get();
 
-            auto oldElementIter = layer.lowerBound(updatedElement.id);
+            auto oldElementIter = layer->lowerBound(updatedElement.id);
             CoinLayerElement* oldElement = nullptr;
-            if (oldElementIter != layer.elements.end() && oldElementIter->data->id == updatedElement.id)
+            if (oldElementIter != layer->elements.end() && oldElementIter->data->id == updatedElement.id)
             {
                 // Element already exists
                 CoinLayerElement* oldElement = &*oldElementIter;
@@ -199,14 +262,14 @@ namespace armarx::viz
                 else
                 {
                     // Types are different, so we delete the old element and create a new one
-                    layer.node->removeChild(oldElementVisu.separator);
+                    layer->node->removeChild(oldElementVisu.separator);
                 }
             }
 
             auto elementVisu = visualizer->create(updatedElement);
             if (elementVisu->separator)
             {
-                layer.node->addChild(elementVisu->separator);
+                layer->node->addChild(elementVisu->separator);
                 if (oldElement)
                 {
                     oldElement->data = updatedElementPtr;
@@ -215,7 +278,7 @@ namespace armarx::viz
                 else
                 {
                     // Need to add a new element
-                    layer.elements.insert(oldElementIter, CoinLayerElement{updatedElementPtr, std::move(elementVisu)});
+                    layer->elements.insert(oldElementIter, CoinLayerElement{updatedElementPtr, std::move(elementVisu)});
                 }
             }
             else
@@ -226,35 +289,6 @@ namespace armarx::viz
                                << "You need to register a visualizer for each type in ArViz/Coin/Visualizer.cpp";
             }
         }
-
-        IceUtil::Time time_updates = IceUtil::Time::now();
-        timing.updateElements = time_updates - time_addLayer;
-
-        // Remove the elements which were not contained in the update
-        for (auto iter = layer.elements.begin(); iter != layer.elements.end();)
-        {
-            coin::ElementVisualization& elementVisu = *iter->visu;
-            if (elementVisu.wasUpdated)
-            {
-                elementVisu.wasUpdated = false;
-                ++iter;
-            }
-            else
-            {
-                layer.node->removeChild(elementVisu.separator);
-                iter = layer.elements.erase(iter);
-            }
-        }
-
-        IceUtil::Time time_remove = IceUtil::Time::now();
-        timing.removeElements = time_remove - time_updates;
-
-        emitLayerUpdated(layerID, layer);
-
-        IceUtil::Time time_end = IceUtil::Time::now();
-        timing.total = time_end - time_start;
-
-        return timing;
     }
 
     void CoinVisualizer::update()
@@ -437,6 +471,57 @@ namespace armarx::viz
         }
     }
 
+    void CoinVisualizer::onSelection(SoPath* path)
+    {
+        SoNode* tailNode = path->getTail();
+
+        // TODO: Store CoinElementID into user data if interaction is enabled!
+        // Search for user data
+        void* userData = tailNode->getUserData();
+        if (userData == nullptr)
+        {
+            // In this case, we want to deselect all objects in the scene,
+            // because the selected object was not marked for selection interactions
+            selection->deselectAll();
+
+            // TODO: Remove logging, this is not an error, but expected
+            ARMARX_INFO << "Selected: " << tailNode->getClassTypeId().getName().getString()
+                        << "\nBut no user data attached!";
+        }
+        CoinElementID* id = static_cast<CoinElementID*>(userData);
+
+        CoinLayer* layer = layers.findLayer(id->layer);
+        if (layer == nullptr)
+        {
+            ARMARX_WARNING << "Selected an element whose layer does not exist: \n"
+                           << "Layer: " << id->layer.first << "/" << id->layer.second
+                           << ", element: " << id->element;
+            return;
+        }
+        CoinLayerElement* element = layer->findElement(id->element);
+        if (element == nullptr)
+        {
+            ARMARX_WARNING << "Selected an element which does not exist: \n"
+                           << "Layer: " << id->layer.first << "/" << id->layer.second
+                           << ", element: " << id->element;
+            return;
+        }
+
+        ARMARX_INFO << "Selected element: \n"
+                    << "Layer: " << id->layer.first << "/" << id->layer.second
+                    << ", element: " << id->element;
+        // TODO: Mark object as selected,
+        //       Store interaction
+        //       Batch send interaction at the end of the main loop?
+    }
+
+    void CoinVisualizer::onDeselection(SoPath* path)
+    {
+        // TODO
+        SoNode* tailNode = path->getTail();
+        ARMARX_INFO << "Deselected: " << tailNode->getClassTypeId().getName().getString();
+    }
+
     void CoinVisualizer::exportToVRML(const std::string& exportFilePath)
     {
 

source/RobotAPI/components/ArViz/Coin/Visualizer.h

@@ -4,7 +4,9 @@
 
 #include <RobotAPI/interface/ArViz/Component.h>
 
+#include <Inventor/nodes/SoSelection.h>
 #include <Inventor/nodes/SoSeparator.h>
+
 #include <IceUtil/Shared.h>
 
 #include <functional>
@@ -184,6 +186,12 @@ namespace armarx::viz
 
     struct CoinVisualizerWrapper;
 
+    struct CoinElementID
+    {
+        CoinLayerID layer;
+        std::string element;
+    };
+
     class CoinVisualizer
     {
     public:
@@ -219,10 +227,20 @@ namespace armarx::viz
 
         CoinVisualizer_UpdateTiming getTiming();
 
+        CoinLayer& findOrAddLayer(CoinLayerID const& layerID);
+        void addOrUpdateElements(CoinLayer* layer, data::LayerUpdate const& update);
+
         std::vector<CoinLayerID> getLayerIDs();
         void emitLayersChanged(std::vector<CoinLayerID> const& layerIDs);
         void emitLayerUpdated(CoinLayerID const& layerID, CoinLayer const& layer);
 
+        void onSelection(SoPath* path);
+        void onDeselection(SoPath* path);
+        // 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<CoinElementID>> selectableElementIDs;
+
         void onUpdateSuccess(data::LayerUpdates const& updates);
         void onUpdateFailure(Ice::Exception const& ex);
         IceUtil::Handle<CoinVisualizerWrapper> callbackData;
@@ -236,6 +254,7 @@ namespace armarx::viz
         std::vector<std::type_index> elementVisualizersTypes;
         std::vector<std::unique_ptr<coin::ElementVisualizer>> elementVisualizers;
 
+        SoSelection* selection = nullptr;
         SoSeparator* root = nullptr;
 
         std::atomic<CoinVisualizerUpdateResult> updateResult{CoinVisualizerUpdateResult::SUCCESS};

source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp

@@ -31,8 +31,6 @@
 #include <QTimer>
 #include <QFileDialog>
 
-#define ENABLE_INTROSPECTION 1
-
 
 namespace armarx
 {
@@ -98,7 +96,6 @@ namespace armarx
 
         connect(widget.layerTree, &QTreeWidget::currentItemChanged, this, &This::updateSelectedLayer);
 
-#if ENABLE_INTROSPECTION
         connect(widget.layerInfoTreeGroupBox, &QGroupBox::toggled, &layerInfoTree, &LayerInfoTree::setEnabled);
         connect(widget.defaultShowLimitSpinBox, qOverload<int>(&QSpinBox::valueChanged),
                 &layerInfoTree, &LayerInfoTree::setMaxElementCountDefault);
@@ -108,8 +105,6 @@ namespace armarx
         layerInfoTree.setEnabled(widget.layerInfoTreeGroupBox->isChecked());
         layerInfoTree.registerVisualizerCallbacks(visualizer);
 
-#endif
-
 
         // We need a callback from the visualizer, when the layers have changed
         // So we can update the tree accordingly
@@ -321,7 +316,6 @@ namespace armarx
 
     void ArVizWidgetController::updateSelectedLayer(QTreeWidgetItem* current, QTreeWidgetItem* previous)
     {
-#if ENABLE_INTROSPECTION
         (void) previous;
 
         if (!current->parent())
@@ -342,8 +336,6 @@ namespace armarx
         {
             layerInfoTree.setSelectedLayer(id, &visualizer.layers);
         }
-
-#endif
     }
 
     void ArVizWidgetController::onCollapseAll(bool)
@@ -1004,7 +996,7 @@ namespace armarx
 
     SoNode* ArVizWidgetController::getScene()
     {
-        return visualizer.root;
+        return visualizer.selection;
     }
 
     static const std::string CONFIG_KEY_STORAGE = "Storage";