diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index 25c01e3c001502c0976296a9c63672d451e48289..aba92019863cdd006d6d09d79e732707e825aeb8 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -94,6 +94,9 @@ namespace armarx
{
auto layerID = std::make_pair(update.component, update.name);
auto layerIt = layers.find(layerID);
+
+ //
+
if (layerIt == layers.end())
{
// Create a new layer
@@ -136,12 +139,6 @@ namespace armarx
switch (updatedElement.action)
{
case Element_ADD:
- {
- // Remove old element and create a new one below
- layer.node->removeChild(oldElement.separator);
- }
- break;
- case Element_UPDATE:
{
bool updated = visualizer->update(updatedElement, &oldElement);
if (updated)
@@ -155,7 +152,8 @@ namespace armarx
}
}
break;
- case Element_DELETE:
+ // FIXME: Remove should be implicit, if the object is not contained in the update
+ case Element_REMOVE:
{
layer.elements.erase(oldElementIter);
layer.node->removeChild(oldElement.separator);
@@ -171,7 +169,7 @@ namespace armarx
}
- if (updatedElement.action == Element_DELETE)
+ if (updatedElement.action == Element_REMOVE)
{
continue;
}
diff --git a/source/RobotAPI/components/ArVizExample/ArVizExample.cpp b/source/RobotAPI/components/ArVizExample/ArVizExample.cpp
index a7df9fd382decfeaef4995fd9cda7c9396f8177b..d1a788d01247d0c7bc11acc87c983a9418bef25e 100644
--- a/source/RobotAPI/components/ArVizExample/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArVizExample/ArVizExample.cpp
@@ -83,31 +83,200 @@ void ArVizExample::onDisconnectComponent()
task = nullptr;
}
+namespace armarx::viz
+{
+
+ class ArVizClient;
+
+ class Box
+ {
+ public:
+ Box(ElementBoxPtr const& data)
+ : data(data)
+ {
+ }
+
+ // TODO: Move to a base class
+ // TODO: add more overloads
+ Box& position(Eigen::Vector3f const& pos)
+ {
+ data->pose.x = pos.x();
+ data->pose.y = pos.y();
+ data->pose.z = pos.z();
+
+ return *this;
+ }
+
+ Box& color(unsigned char r, unsigned char g, unsigned char b)
+ {
+ data->color = Color{255, r, g, b};
+
+ return *this;
+ }
+
+ Box& size(Eigen::Vector3f const& s)
+ {
+ data->size.e0 = s.x();
+ data->size.e1 = s.y();
+ data->size.e2 = s.z();
+
+ return *this;
+ }
+
+
+ private:
+ ElementBoxPtr data;
+ };
+
+ class Layer
+ {
+ public:
+ Layer(ArVizClient* client, std::size_t updateIndex)
+ : client(client)
+ , updateIndex(updateIndex)
+ {
+ }
+
+ void clear();
+
+ Box box(std::string const& id);
+
+ private:
+ ArVizClient* client = nullptr;
+ std::size_t updateIndex = 0;
+ };
+
+ class ArVizClient
+ {
+ public:
+ ArVizClient(armarx::Component& component)
+ {
+ componentName = component.getName();
+
+ // Should this name be configurable?
+ topic = component.getTopic<decltype(topic)>("ArVizTopic");
+ }
+
+ Layer layer(std::string const& name)
+ {
+ LayerUpdate* update = nullptr;
+ for (auto& layer : layers)
+ {
+ if (layer.name == name)
+ {
+ update = &layer;
+ break;
+ }
+ }
+ if (update == nullptr)
+ {
+ // Create new layer
+ auto& newLayer = layers.emplace_back();
+ newLayer.component = componentName;
+ newLayer.name = name;
+ newLayer.action = Layer_CREATE_OR_UPDATE;
+ update = &newLayer;
+ }
+
+ std::size_t updateIndex = update - layers.data();
+ }
+
+ void clearLayer(std::size_t layerIndex)
+ {
+ if (layerIndex < layers.size())
+ {
+ // TODO: Rename action?
+ layers[layerIndex].action = Layer_DELETE;
+
+ }
+ }
+
+ void commit()
+ {
+ // Send changes to topic
+ topic->updateLayers(layers);
+ }
+
+ template <typename ElementT>
+ IceInternal::Handle<ElementT> addElement(std::size_t layerIndex, std::string const& id)
+ {
+ IceInternal::Handle<ElementT> newElement(new ElementT);
+ newElement->id = id;
+ if (layerIndex < layers.size())
+ {
+ // Must be cleared sometime?
+ layers[layerIndex].elements.push_back(newElement);
+ }
+ else
+ {
+ ARMARX_WARNING << "Layer index is invalid: " << layerIndex << " >= " << layers.size();
+ }
+ return newElement;
+ }
+
+ private:
+ std::string componentName;
+ armarx::viz::TopicPrx topic;
+
+ LayerUpdateSeq layers;
+ };
+
+ void Layer::clear()
+ {
+ client->clearLayer(updateIndex);
+ }
+
+ Box Layer::box(const std::string& id)
+ {
+ ElementBoxPtr data = client->addElement<ElementBox>(updateIndex, id);
+ return Box(data);
+ }
+
+}
+
+
+
void ArVizExample::update()
{
+ viz::ArVizClient arviz(*this);
+
+ // Create objects to represent each layer
+ viz::Layer testLayer = arviz.layer("Test");
+ viz::Layer exampleLayer = arviz.layer("Example");
+
CycleUtil c(20);
while (!task->isStopped())
{
double timeInSeconds = TimeUtil::GetTime().toSecondsDouble();
+ // Improve the client side code
+ // We need wrappers for ArViz, layer, element management
{
viz::LayerUpdate layer;
layer.component = getName();
layer.name = "Test";
- {
- viz::ElementBoxPtr box(new viz::ElementBox);
- box->id = "box";
- box->action = viz::Element_UPDATE;
-
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.x() = 100.0f * std::sin(timeInSeconds);
- box->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
- box->color = armarx::viz::Color{255, 255, 0, 0};
- box->size = frost(Eigen::Vector3f(100.0f, 100.0f, 100.0f));
-
- layer.elements.push_back(box);
- }
+ // Add a box to testLayer
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.x() = 100.0f * std::sin(timeInSeconds);
+ viz::Box box = testLayer.box("box")
+ .position(pos)
+ .color(255, 0, 0)
+ .size(Eigen::Vector3f(100.0f, 100.0f, 100.0f));
+
+ // {
+ // viz::ElementBoxPtr box(new viz::ElementBox);
+ // box->id = "box";
+ // box->action = viz::Element_UPDATE;
+
+ // Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ // pos.x() = 100.0f * std::sin(timeInSeconds);
+ // box->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
+ // box->color = armarx::viz::Color{255, 255, 0, 0};
+ // box->size = frost(Eigen::Vector3f(100.0f, 100.0f, 100.0f));
+
+ // layer.elements.push_back(box);
+ // }
{
viz::ElementCylinderPtr cyl(new viz::ElementCylinder);
cyl->id = "cylinder";
@@ -185,6 +354,7 @@ void ArVizExample::update()
layer.elements.push_back(arrow);
}
topic->updateLayers({layer});
+ arviz.commit();
}
{
viz::LayerUpdate layer;
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index df517e280a2e535a9617678d3e1be008739ce470..415ab9c6ee43488661f05f383b4467e9ec9068a6 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -120,15 +120,17 @@ void ArVizWidgetController::layersChanged(std::vector<viz::CoinLayerID> const& l
std::map<std::string, QTreeWidgetItem*> currentComponents;
std::map<viz::CoinLayerID, QTreeWidgetItem*> currentLayers;
- for (QTreeWidgetItemIterator compIter(widget.layerTree); *compIter; ++compIter)
+ int componentCount = widget.layerTree->topLevelItemCount();
+ for (int compIndex = 0; compIndex < componentCount; ++compIndex)
{
- QTreeWidgetItem* componentItem = *compIter;
+ QTreeWidgetItem* componentItem = widget.layerTree->topLevelItem(compIndex);
std::string component = componentItem->text(0).toStdString();
currentComponents.emplace(component, componentItem);
- for (QTreeWidgetItemIterator layerIter(componentItem); *layerIter; ++layerIter)
+ int layerCount = componentItem->childCount();
+ for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex)
{
- QTreeWidgetItem* layerItem = *layerIter;
+ QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
std::string layer = layerItem->text(0).toStdString();
viz::CoinLayerID layerID(component, layer);
diff --git a/source/RobotAPI/interface/ArViz/Elements.ice b/source/RobotAPI/interface/ArViz/Elements.ice
index 5001d3c393c47b76dea4a99689a7001f5ceddfad..fc852f723f629d8da14eb0ae20cd0e13311f0658 100644
--- a/source/RobotAPI/interface/ArViz/Elements.ice
+++ b/source/RobotAPI/interface/ArViz/Elements.ice
@@ -30,9 +30,8 @@ module viz
enum ElementAction
{
- Element_ADD = 0,
- Element_UPDATE = 1,
- Element_DELETE = 2,
+ Element_ADD,
+ Element_REMOVE,
};
class Element