diff --git a/.gitlab/CODEOWNERS b/.gitlab/CODEOWNERS
index 88113c1408093d1bd0bd42f4d703bfb7f67ae663..a50d21fe1c21109729acf45e579328b68e740f75 100644
--- a/.gitlab/CODEOWNERS
+++ b/.gitlab/CODEOWNERS
@@ -1,9 +1,10 @@
# ArMem
-/source/RobotAPI/components/armem/ @RainerKartmann @dreher
+/source/RobotAPI/components/armem/ @fratty @RainerKartmann @dreher
-/source/RobotAPI/libraries/armem/ @RainerKartmann @dreher
+/source/RobotAPI/libraries/armem/ @fratty @RainerKartmann @dreher
/source/RobotAPI/libraries/armem_robot_localization/ @FabianReister
/source/RobotAPI/libraries/armem_robot_mapping/ @FabianReister
-/source/RobotAPI/interface/armem.ice @RainerKartmann @dreher
+/source/RobotAPI/interface/armem/ @fratty @RainerKartmann @dreher
+/source/RobotAPI/interface/armem.ice @fratty @RainerKartmann @dreher
diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index 5a21a1cd4e02935799bdd4788aa7e2cac5ca12c8..d79d9a4ba4e3ee75d0860328997112e864c008d3 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -15,6 +15,7 @@ set(SOURCES
Client/elements/Mesh.cpp
Client/elements/Robot.cpp
Client/elements/RobotHand.cpp
+ Client/drawer/ArVizDrawerBase.cpp
Coin/ElementVisualizer.cpp
@@ -75,6 +76,8 @@ set(HEADERS
Client/elements/Robot.h
Client/elements/RobotHand.h
+ Client/drawer/ArVizDrawerBase.h
+
Client/elements/point_cloud_type_traits.hpp
Introspection/ElementJsonSerializers.h
diff --git a/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.cpp b/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a19032cd088ccdf49e16af5ea394d73a92b30878
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.cpp
@@ -0,0 +1,29 @@
+#include "ArVizDrawerBase.h"
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+#include "RobotAPI/components/ArViz/Client/Layer.h"
+
+namespace armarx
+{
+
+ ArVizDrawerBase::ArVizDrawerBase(armarx::viz::Client &arviz) : arvizClient(arviz) {}
+
+ ArVizDrawerBase::~ArVizDrawerBase() = default;
+
+ viz::Client &ArVizDrawerBase::arviz() { return arvizClient; }
+
+ const viz::Client &ArVizDrawerBase::arviz() const { return arvizClient; }
+
+ void ArVizDrawerBase::commit(const viz::Layer &layer)
+ {
+ std::lock_guard guard{layerMtx};
+ arvizClient.commit(layer);
+ }
+
+ void ArVizDrawerBase::commit(const std::vector<viz::Layer> &layers)
+ {
+ std::lock_guard guard{layerMtx};
+ arvizClient.commit(layers);
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.h b/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..f7c813be72283ec4899f402a1133c41ab5a381e2
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/drawer/ArVizDrawerBase.h
@@ -0,0 +1,62 @@
+/*
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+#include <vector>
+
+namespace armarx
+{
+ // forward declaration
+ namespace viz
+ {
+ class Client;
+ struct Layer;
+ } // namespace viz
+
+ /**
+ * @brief The ArVizDrawerBase class. Use this class to draw arbitrary objects.
+ *
+ * Instead of implementing arviz drawing in the component itself, this class helps to
+ * decouple drawing from "component / processing logic".
+ *
+ */
+ class ArVizDrawerBase
+ {
+ public:
+ ArVizDrawerBase(armarx::viz::Client &arviz);
+ virtual ~ArVizDrawerBase();
+
+ protected:
+ viz::Client &arviz();
+ const viz::Client &arviz() const;
+
+ void commit(const viz::Layer &layer);
+ void commit(const std::vector<viz::Layer> &layers);
+
+ private:
+ viz::Client &arvizClient;
+
+ std::mutex layerMtx;
+ };
+
+} // namespace armarx
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..5cb71b0fecc568e06b42ae49d8f8e9525104feea 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
@@ -5,14 +5,13 @@
#include <RobotAPI/interface/ArViz/Component.h>
#include <Inventor/nodes/SoSeparator.h>
+#include <IceUtil/Shared.h>
-#include <map>
-#include <unordered_map>
+#include <functional>
+#include <typeindex>
#include <memory>
+#include <string>
#include <mutex>
-#include <typeindex>
-
-#include <IceUtil/Shared.h>
namespace armarx::viz
{
@@ -25,19 +24,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 +66,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 +225,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/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
index 5ed005e7a82c5c5eb24b056d0f5d71e10ebcf380..c1ebb7ba455032cf3d48fa32b40081a6a2fcdb3d 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.cpp
@@ -22,6 +22,8 @@
#include "ExampleMemoryClient.h"
+#include <random>
+
#include <SimoxUtility/color/cmaps.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -112,6 +114,8 @@ namespace armarx
void ExampleMemoryClient::run()
{
ARMARX_IMPORTANT << "Running example.";
+ run_started = IceUtil::Time::now();
+ std::srand(std::time(nullptr));
armem::MemoryID snapshotID = commitSingleSnapshot(entityID);
if (true)
@@ -132,11 +136,15 @@ namespace armarx
queryExampleData();
}
- CycleUtil c(1000);
+ CycleUtil c(100);
int i = 0;
- while (!task->isStopped() && i++ < 100)
+ while (!task->isStopped())
{
- commitSingleSnapshot(entityID);
+ commitExampleData();
+ if (i++ < 10000)
+ {
+ commitSingleSnapshot(entityID);
+ }
c.waitForCycleDuration();
}
}
@@ -161,10 +169,30 @@ namespace armarx
armem::EntityUpdate update;
update.entityID = entityID;
update.timeCreated = armem::Time::now();
+
+ double diff = (update.timeCreated - run_started).toMilliSecondsDouble() / 1000;
+
+ auto dict1 = std::make_shared<aron::datanavigator::DictNavigator>();
+ auto dict2 = std::make_shared<aron::datanavigator::DictNavigator>();
+
+ auto sin = std::make_shared<aron::datanavigator::FloatNavigator>(std::sin(diff));
+ auto cos = std::make_shared<aron::datanavigator::FloatNavigator>(std::cos(diff));
+
+ auto sqrt = std::make_shared<aron::datanavigator::DoubleNavigator>(std::sqrt(diff));
+ auto lin = std::make_shared<aron::datanavigator::LongNavigator>((long)(diff * 1000));
+ auto rand = std::make_shared<aron::datanavigator::IntNavigator>(std::rand());
+
+ dict1->addElement("sin", sin);
+ dict1->addElement("cos", cos);
+
+ dict2->addElement("sqrt", sqrt);
+ dict2->addElement("lin", lin);
+ dict2->addElement("rand", rand);
+
update.instancesData =
{
- std::make_shared<aron::datanavigator::DictNavigator>(),
- std::make_shared<aron::datanavigator::DictNavigator>()
+ dict1,
+ dict2
};
ARMARX_IMPORTANT << "Committing " << update;
@@ -311,6 +339,7 @@ namespace armarx
armem::Time time = armem::Time::now();
armem::Commit commit;
+ //default
{
armem::EntityUpdate& update = commit.updates.emplace_back();
update.entityID = providerID.withEntityName("default");
@@ -319,6 +348,7 @@ namespace armarx
armem::example::ExampleData primitive;
update.instancesData = { primitive.toAron() };
}
+ //the answer
{
armem::EntityUpdate& update = commit.updates.emplace_back();
update.entityID = providerID.withEntityName("the answer");
@@ -326,7 +356,7 @@ namespace armarx
armem::example::ExampleData data;
data.the_bool = true;
- data.the_double = 2 * 42.42;
+ data.the_double = std::sin(time.toSecondsDouble());
data.the_float = 21.5;
data.the_int = 42;
data.the_long = 424242;
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
index e2e975c4a5450fa614cf0020b1e21a09fabbdb4f..774a4cbb84bc06496471e214d1cdb7935af1418f 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
@@ -114,6 +114,8 @@ namespace armarx
private:
+ IceUtil::Time run_started;
+
armarx::RunningTask<ExampleMemoryClient>::pointer_type task;
armarx::DebugObserverInterfacePrx debugObserver;
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/ArVizWidgetController.h b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
index e59b685797f5e8ee4b56220c9c5070109e50a140..7a3a0bfa34330ecf32509b24d2e1d420e4016d0f 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
@@ -215,6 +215,9 @@ namespace armarx
IceUtil::Handle<ArVizWidgetBatchCallback> callbackData;
armarx::viz::Callback_StorageInterface_getRecordingBatchPtr callback;
Ice::AsyncResultPtr callbackResult;
+
+ std::vector<double> timings;
+
public:
static QIcon GetWidgetIcon()
{
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;
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 2f039d60c1343421a5a22bc576eec0ea7b3fc456..38cdb96be1a6e47b98e7adc0c939cc816cfd55c6 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -340,6 +340,7 @@ module armarx
void resetDMP();
void stopDMP();
void resumeDMP();
+ void removeAllViaPoints();
void setMPWeights(DoubleSeqSeq weights);
DoubleSeqSeq getMPWeights();
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 0ed7f2136377c71399c71fec0310fbec12baba85..70fae3d0b1c6576d26d71ee652cd79ef4ae3c75d 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -575,6 +575,12 @@ namespace armarx
return resvec;
}
+ void NJointTaskSpaceImpedanceDMPController::removeAllViaPoints(const Ice::Current&)
+ {
+ LockGuardType guard {controllerMutex};
+ ARMARX_INFO << "setting via points ";
+ dmpCtrl->removeAllViaPoints();
+ }
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index 412a46f20b5ce93eddfed0d68854d4a3c03bc30a..34bd9785cbac94d173e45c6e345cf1531e0a158c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -74,6 +74,8 @@ namespace armarx
void setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&);
DoubleSeqSeq getMPWeights(const Ice::Current&);
+ void removeAllViaPoints(const Ice::Current&) override;
+
protected:
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);