diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index 89c8340b7fd524b9d3df06bb4332e7e591701011..96ad7fed33e9be2813679f6ab20405f85300fc57 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -144,24 +144,25 @@ namespace armarx::viz
CoinVisualizer_ApplyTiming timing;
timing.layerName = update.name;
- auto layerID = std::make_pair(update.component, update.name);
- auto layerIt = layers.find(layerID);
+ CoinLayerID layerID(update.component, update.name);
+ auto layerIt = layers.lowerBound(layerID);
- if (layerIt == layers.end())
+ if (layerIt == layers.data.end() || layerIt->id != layerID)
{
// Create a new layer
SoSeparator* coinNode = new SoSeparator;
coinNode->ref();
root->addChild(coinNode);
- layerIt = layers.emplace(layerID, CoinLayer(coinNode)).first;
+ layerIt = layers.data.insert(layerIt, CoinLayer(layerID, coinNode));
}
IceUtil::Time time_addLayer = IceUtil::Time::now();
timing.addLayer = time_addLayer - time_start;
// Add or update the elements in the update
- CoinLayer& layer = layerIt->second;
+ CoinLayer& layer = *layerIt;
+ layer.elements.reserve(update.elements.size());
for (auto& updatedElementPtr : update.elements)
{
if (!updatedElementPtr)
@@ -191,24 +192,27 @@ namespace armarx::viz
}
coin::ElementVisualizer* visualizer = elementVisualizers[visuIndex].get();
- auto oldElementIter = layer.elements.find(updatedElement.id);
- if (oldElementIter != layer.elements.end())
+ auto oldElementIter = layer.lowerBound(updatedElement.id);
+ CoinLayerElement* oldElement = nullptr;
+ if (oldElementIter != layer.elements.end() && oldElementIter->data->id == updatedElement.id)
{
// Element already exists
- coin::ElementVisualization& oldElement = *oldElementIter->second.visu;
- oldElement.wasUpdated = true;
+ CoinLayerElement* oldElement = &*oldElementIter;
+ coin::ElementVisualization& oldElementVisu = *oldElement->visu;
- bool updated = visualizer->update(updatedElement, &oldElement);
+ oldElementVisu.wasUpdated = true;
+
+ bool updated = visualizer->update(updatedElement, &oldElementVisu);
if (updated)
{
- layer.elements[updatedElement.id].data = updatedElementPtr;
+ oldElement->data = updatedElementPtr;
continue;
}
else
{
// Types are different, so we delete the old element and create a new one
- layer.node->removeChild(oldElement.separator);
+ layer.node->removeChild(oldElementVisu.separator);
}
}
@@ -216,7 +220,16 @@ namespace armarx::viz
if (elementVisu->separator)
{
layer.node->addChild(elementVisu->separator);
- layer.elements[updatedElement.id] = CoinLayerElement{updatedElementPtr, std::move(elementVisu)};
+ if (oldElement)
+ {
+ oldElement->data = updatedElementPtr;
+ oldElement->visu = std::move(elementVisu);
+ }
+ else
+ {
+ // Need to add a new element
+ layer.elements.insert(oldElementIter, CoinLayerElement{updatedElementPtr, std::move(elementVisu)});
+ }
}
else
{
@@ -233,7 +246,7 @@ namespace armarx::viz
// Remove the elements which were not contained in the update
for (auto iter = layer.elements.begin(); iter != layer.elements.end();)
{
- auto& elementVisu = *iter->second.visu;
+ coin::ElementVisualization& elementVisu = *iter->visu;
if (elementVisu.wasUpdated)
{
elementVisu.wasUpdated = false;
@@ -266,7 +279,7 @@ namespace armarx::viz
std::unique_lock<std::mutex> lock(stateMutex);
storage = stateStorage;
root->removeAllChildren();
- layers.clear();
+ layers.data.clear();
pulledUpdates.revision = 0;
pulledUpdates.updates.clear();
updateResult = CoinVisualizerUpdateResult::SUCCESS;
@@ -364,20 +377,19 @@ namespace armarx::viz
void CoinVisualizer::showLayer(CoinLayerID const& id, bool visible)
{
- auto iter = layers.find(id);
- if (iter == layers.end())
+ CoinLayer* layer = layers.findLayer(id);
+ if (layer == nullptr)
{
return;
}
- viz::CoinLayer& layer = iter->second;
- int childIndex = root->findChild(layer.node);
+ int childIndex = root->findChild(layer->node);
if (childIndex < 0)
{
// Layer is currently not visible
if (visible)
{
- root->addChild(layer.node);
+ root->addChild(layer->node);
}
}
else
@@ -399,10 +411,10 @@ namespace armarx::viz
std::vector<CoinLayerID> CoinVisualizer::getLayerIDs()
{
std::vector<CoinLayerID> result;
- result.reserve(layers.size());
- for (auto& entry : layers)
+ result.reserve(layers.data.size());
+ for (CoinLayer& layer : layers.data)
{
- result.push_back(entry.first);
+ result.push_back(layer.id);
}
return result;
}
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.h b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
index 01d7072ca3a59dc6b69fb6b5c0adf3e6f25b4ead..806099952dd0d5043c4bfaeed9c9c358870f63e1 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
@@ -25,19 +25,31 @@ namespace armarx::viz
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(SoSeparator* node)
- : node(node)
+ 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)
- : node(other.node), elements(std::move(other.elements))
+ : id(std::move(other.id))
+ , node(other.node)
+ , elements(std::move(other.elements))
+ , pivot(std::move(other.pivot))
{
other.node = nullptr;
}
@@ -55,13 +67,69 @@ namespace armarx::viz
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::map<std::string, CoinLayerElement> elements;
+ 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
@@ -158,7 +226,7 @@ namespace armarx::viz
viz::StorageInterfacePrx storage;
SoTimerSensor* timerSensor = nullptr;
- std::map<CoinLayerID, CoinLayer> layers;
+ CoinLayerMap layers;
std::vector<std::type_index> elementVisualizersTypes;
std::vector<std::unique_ptr<coin::ElementVisualizer>> elementVisualizers;
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index cf42b9ff2bb4d42f04a6067f08803ea6ac69f873..44d9c0aa5a71bee4a4095077a3aca2525cb0437a 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -171,7 +171,7 @@ namespace armarx
emit connectGui();
}
- void armarx::ArVizWidgetController::onDisconnectComponent()
+ void ArVizWidgetController::onDisconnectComponent()
{
visualizer.stop();
@@ -336,17 +336,18 @@ namespace armarx
}
// A layer was selected.
- const viz::CoinLayerID id = std::make_pair(current->parent()->text(0).toStdString(), current->text(0).toStdString());
+ viz::CoinLayerID id(current->parent()->text(0).toStdString(), current->text(0).toStdString());
- try
+ viz::CoinLayer* layer = visualizer.layers.findLayer(id);
+ if (layer == nullptr)
{
- viz::CoinLayer& layer = visualizer.layers.at(id);
- layerInfoTree.setSelectedLayer(id, &layer);
+ ARMARX_WARNING << "Could not find layer (" << id.first << " / " << id.second << ") in Visualizer.";
}
- catch (const std::out_of_range&)
+ else
{
- ARMARX_WARNING << "Could not find layer (" << id.first << " / " << id.second << ") in Visualizer.";
+ layerInfoTree.setSelectedLayer(id, &visualizer.layers);
}
+
#endif
}
@@ -446,10 +447,22 @@ namespace armarx
{
if (debugObserver)
{
- timingMap["0.pull"] = new Variant(timing.pull.toMilliSecondsDouble());
- timingMap["1.apply"] = new Variant(timing.applyTotal.total.toMilliSecondsDouble());
- timingMap["2.layers"] = new Variant(timing.layersChanged.toMilliSecondsDouble());
- timingMap["total"] = new Variant(timing.total.toMilliSecondsDouble());
+ timingMap["0. pull (ms)"] = new Variant(timing.pull.toMilliSecondsDouble());
+ timingMap["1. apply (ms)"] = new Variant(timing.applyTotal.total.toMilliSecondsDouble());
+ timingMap["1.1 apply, addLayer (ms)"] = new Variant(timing.applyTotal.addLayer.toMilliSecondsDouble());
+ timingMap["1.2 apply, updateElements (ms)"] = new Variant(timing.applyTotal.updateElements.toMilliSecondsDouble());
+ timingMap["1.3 apply, removeElements (ms)"] = new Variant(timing.applyTotal.removeElements.toMilliSecondsDouble());
+ timingMap["2. layers (ms)"] = new Variant(timing.layersChanged.toMilliSecondsDouble());
+ timingMap["total (ms)"] = new Variant(timing.total.toMilliSecondsDouble());
+
+ timings.push_back(timing.total.toMilliSecondsDouble());
+ int numTimings = 20;
+ if ((int)timings.size() > numTimings)
+ {
+ timings.erase(timings.begin());
+ }
+ double averageTime = std::accumulate(timings.begin(), timings.end(), 0.0) / numTimings;
+ timingMap["total avg (ms)"] = new Variant(averageTime);
debugObserver->begin_setDebugChannel("ArViz_Timing", timingMap);
}
@@ -920,7 +933,7 @@ namespace armarx
}
- auto& entry = iter->second;
+ TimestampedRecordingBatch& entry = iter->second;
entry.lastUsed = now;
return entry.data;
}
@@ -939,7 +952,7 @@ namespace armarx
for (auto iter = recordingBatchCache.begin();
iter != recordingBatchCache.end(); ++iter)
{
- auto& entry = iter->second;
+ TimestampedRecordingBatch& entry = iter->second;
if (entry.lastUsed < oldestIter->second.lastUsed)
{
oldestIter = iter;
@@ -957,8 +970,8 @@ namespace armarx
return visualizer.root;
}
- const static std::string CONFIG_KEY_STORAGE = "Storage";
- const static std::string CONFIG_KEY_DEBUG_OBSERVER = "DebugObserver";
+ static const std::string CONFIG_KEY_STORAGE = "Storage";
+ static const std::string CONFIG_KEY_DEBUG_OBSERVER = "DebugObserver";
void ArVizWidgetController::loadSettings(QSettings* settings)
{
@@ -976,7 +989,7 @@ namespace armarx
QString::fromStdString(debugObserverName));
}
- QPointer<QDialog> armarx::ArVizWidgetController::getConfigDialog(QWidget* parent)
+ QPointer<QDialog> ArVizWidgetController::getConfigDialog(QWidget* parent)
{
if (!configDialog)
{
@@ -987,7 +1000,7 @@ namespace armarx
return qobject_cast<QDialog*>(configDialog);
}
- void armarx::ArVizWidgetController::configured()
+ void ArVizWidgetController::configured()
{
if (configDialog)
{
diff --git a/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.cpp b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.cpp
index 9d78b9888742040908574c587a8061bc50c2b61a..2d20d7974a53aae2ebfa73d053b9d7a1a86b4439 100644
--- a/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.cpp
@@ -40,9 +40,20 @@ namespace armarx
// Update layer element properties when expanded.
widgetConnections.append(connect(widget, &QTreeWidget::itemExpanded, [this](QTreeWidgetItem * item)
{
- if (!item->parent())
+ if (item->parent() || this->layers == nullptr)
{
- this->updateLayerElementProperties(item, this->layer->elements.at(item->text(0).toStdString()).data);
+ return;
+ }
+
+ std::string id = item->text(0).toStdString();
+ auto* layer = this->layers->findLayer(this->layerID);
+ if (layer != nullptr)
+ {
+ viz::CoinLayerElement const* element = layer->findElement(id);
+ if (element != nullptr)
+ {
+ this->updateLayerElementProperties(item, element->data);
+ }
}
}));
@@ -58,10 +69,6 @@ namespace armarx
{
if (layerID == this->layerID)
{
- // If the layer object identified by layerID changed, we should use the new one.
- // This can happen when the gui plugin reconnects.
- this->layer = &layer;
-
this->update();
}
});
@@ -88,29 +95,29 @@ namespace armarx
}
}
- void LayerInfoTree::setSelectedLayer(viz::CoinLayerID id, viz::CoinLayer* layer)
+ void LayerInfoTree::setSelectedLayer(viz::CoinLayerID id, viz::CoinLayerMap* layers)
{
this->layerID = id;
- this->layer = layer;
+ this->layers = layers;
showMoreItem = nullptr;
maxElementCount = maxElementCountDefault;
widget->clear();
- if (!layer)
- {
- return;
- }
update();
}
void LayerInfoTree::update()
{
- if (enabled && layer)
+ if (enabled && layers)
{
- updateLayerElements(layer->elements);
+ auto* layer = layers->findLayer(layerID);
+ if (layer != nullptr)
+ {
+ updateLayerElements(layer->elements);
+ }
}
}
@@ -125,7 +132,7 @@ namespace armarx
}
- void LayerInfoTree::updateLayerElements(const std::map<std::string, viz::CoinLayerElement>& elements)
+ void LayerInfoTree::updateLayerElements(const std::vector<viz::CoinLayerElement>& elements)
{
if (showMoreItem)
{
@@ -139,9 +146,9 @@ namespace armarx
ARMARX_CHECK_IS_NULL(showMoreItem);
int currentIndex = 0;
- for (auto it = elements.begin(); it != elements.end(); ++it)
+ for (viz::CoinLayerElement const& element : elements)
{
- const auto& [name, element] = *it;
+ std::string const& name = element.data->id;
if (maxElementCount >= 0 && currentIndex >= maxElementCount)
{
@@ -546,6 +553,14 @@ namespace armarx
{
ARMARX_CHECK_NULL(showMoreItem) << "There should not be a showMoreItem when this function is called.";
+ auto* layer = this->layers->findLayer(this->layerID);
+ if (layer == nullptr)
+ {
+ ARMARX_WARNING << "No layer with ID '" << this->layerID.first
+ << "/" << this->layerID.second << "' found";
+ return;
+ }
+
std::stringstream ss;
ss << "Showing " << widget->topLevelItemCount() << " of " << layer->elements.size() << " elements.";
@@ -556,7 +571,7 @@ namespace armarx
QPushButton* button = new QPushButton("Show more");
widget->setItemWidget(item, 0, button);
- connect(button, &QPushButton::pressed, [this]()
+ connect(button, &QPushButton::pressed, [this, layer]()
{
maxElementCount += maxElementCountDefault;
this->updateLayerElements(layer->elements);
diff --git a/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.h b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.h
index a5289ac8b10aa7a7d184c3e4922c26fed5dabcc4..bccbff58de0ad18bfbd289f0d08ac21d69419bfb 100644
--- a/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.h
+++ b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.h
@@ -35,7 +35,7 @@ namespace armarx
void setEnabled(bool enabled);
/// Set the selected layer.
- void setSelectedLayer(viz::CoinLayerID id, viz::CoinLayer* layer);
+ void setSelectedLayer(viz::CoinLayerID id, viz::CoinLayerMap* layers);
/// Update the currently expanded layer elements.
void update();
@@ -57,7 +57,7 @@ namespace armarx
*
* Items are inserted, removed and updated accordingly.
*/
- void updateLayerElements(const std::map<std::string, viz::CoinLayerElement>& elements);
+ void updateLayerElements(const std::vector<viz::CoinLayerElement>& elements);
/// Insert a (top-level) layer element at `ìndex`.
QTreeWidgetItem* insertLayerElementItem(int index, const std::string& name, const viz::data::ElementPtr& element);
@@ -122,7 +122,7 @@ namespace armarx
int maxElementCount = maxElementCountDefault;
viz::CoinLayerID layerID = {"", ""};
- const viz::CoinLayer* layer = nullptr;
+ const viz::CoinLayerMap* layers = nullptr;