diff --git a/source/RobotAPI/components/ArVizExample/ArVizExample.cpp b/source/RobotAPI/components/ArVizExample/ArVizExample.cpp
index 80a2202cac30e6e5009d38c6bffcc83174c8c157..a7df9fd382decfeaef4995fd9cda7c9396f8177b 100644
--- a/source/RobotAPI/components/ArVizExample/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArVizExample/ArVizExample.cpp
@@ -90,228 +90,235 @@ void ArVizExample::update()
{
double timeInSeconds = TimeUtil::GetTime().toSecondsDouble();
- viz::LayerUpdate layer;
- layer.component = getName();
- layer.name = "Example";
-#if 0
-#endif
- {
- 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";
- cyl->action = viz::Element_UPDATE;
-
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.y() = 100.0f * std::sin(timeInSeconds);
- pos.x() = 150.0f;
- cyl->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
- cyl->color = armarx::viz::Color{255, 0, 255, 0};
- cyl->radius = 50.0f;
- cyl->height = 100.0f;
-
- layer.elements.push_back(cyl);
- }
+ viz::LayerUpdate layer;
+ layer.component = getName();
+ layer.name = "Test";
- {
- viz::ElementPosePtr pose(new viz::ElementPose);
- pose->id = "pose";
- pose->action = viz::Element_UPDATE;
-
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = 100.0f * std::sin(timeInSeconds);
- pos.x() = -150.0f;
- pose->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
- pose->color = armarx::viz::Color{255, 0, 0, 0};
- pose->scale = 1.0f;
-
- layer.elements.push_back(pose);
- }
- {
- viz::ElementTextPtr text(new viz::ElementText);
- text->id = "text";
- text->action = viz::Element_UPDATE;
+ {
+ viz::ElementBoxPtr box(new viz::ElementBox);
+ box->id = "box";
+ box->action = viz::Element_UPDATE;
- text->text = "Test Text";
- text->scale = 4.0f;
+ 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));
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = +300.0f;
- text->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
- text->color = armarx::viz::Color{255, 0, 0, 0};
+ layer.elements.push_back(box);
+ }
+ {
+ viz::ElementCylinderPtr cyl(new viz::ElementCylinder);
+ cyl->id = "cylinder";
+ cyl->action = viz::Element_UPDATE;
+
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.y() = 100.0f * std::sin(timeInSeconds);
+ pos.x() = 150.0f;
+ cyl->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
+ cyl->color = armarx::viz::Color{255, 0, 255, 0};
+ cyl->radius = 50.0f;
+ cyl->height = 100.0f;
+
+ layer.elements.push_back(cyl);
+ }
- layer.elements.push_back(text);
- }
- {
- viz::ElementArrowPtr arrow(new viz::ElementArrow);
- arrow->id = "arrow";
- arrow->action = viz::Element_UPDATE;
-
- float modTime = std::fmod(timeInSeconds, 2.0 * M_PI);
- arrow->length = 200.0f + 100.0f * std::sin(modTime);
-
- Eigen::AngleAxisf dirRot(modTime, Eigen::Vector3f::UnitZ());
- Eigen::Vector3f direction = dirRot * Eigen::Vector3f::UnitX();
- // Calculate rotation to achieve direction
- Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitY();
- Eigen::Vector3f cross = naturalDir.cross(direction);
- float angle = std::acos(naturalDir.dot(direction));
- if (cross.squaredNorm() < 1.0e-12)
{
- // Directions are almost colinear ==> Do no rotation
- cross = Eigen::Vector3f::UnitX();
- angle = 0.0f;
+ viz::ElementPosePtr pose(new viz::ElementPose);
+ pose->id = "pose";
+ pose->action = viz::Element_UPDATE;
+
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.z() = 100.0f * std::sin(timeInSeconds);
+ pos.x() = -150.0f;
+ pose->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
+ pose->color = armarx::viz::Color{255, 0, 0, 0};
+ pose->scale = 1.0f;
+
+ layer.elements.push_back(pose);
}
- Eigen::Vector3f axis = cross.normalized();
- Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
+ {
+ viz::ElementTextPtr text(new viz::ElementText);
+ text->id = "text";
+ text->action = viz::Element_UPDATE;
+
+ text->text = "Test Text";
+ text->scale = 4.0f;
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = +300.0f;
- pos.x() = -500.0f;
- arrow->pose = makeGlobalPose(pos, ori);
- arrow->color = armarx::viz::Color{255, 0, 0, 255};
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.z() = +300.0f;
+ text->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
+ text->color = armarx::viz::Color{255, 0, 0, 0};
- layer.elements.push_back(arrow);
+ layer.elements.push_back(text);
+ }
+ {
+ viz::ElementArrowPtr arrow(new viz::ElementArrow);
+ arrow->id = "arrow";
+ arrow->action = viz::Element_UPDATE;
+
+ float modTime = std::fmod(timeInSeconds, 2.0 * M_PI);
+ arrow->length = 200.0f + 100.0f * std::sin(modTime);
+
+ Eigen::AngleAxisf dirRot(modTime, Eigen::Vector3f::UnitZ());
+ Eigen::Vector3f direction = dirRot * Eigen::Vector3f::UnitX();
+ // Calculate rotation to achieve direction
+ Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitY();
+ Eigen::Vector3f cross = naturalDir.cross(direction);
+ float angle = std::acos(naturalDir.dot(direction));
+ if (cross.squaredNorm() < 1.0e-12)
+ {
+ // Directions are almost colinear ==> Do no rotation
+ cross = Eigen::Vector3f::UnitX();
+ angle = 0.0f;
+ }
+ Eigen::Vector3f axis = cross.normalized();
+ Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
+
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.z() = +300.0f;
+ pos.x() = -500.0f;
+ arrow->pose = makeGlobalPose(pos, ori);
+ arrow->color = armarx::viz::Color{255, 0, 0, 255};
+
+ layer.elements.push_back(arrow);
+ }
+ topic->updateLayers({layer});
}
{
- viz::ElementArrowCirclePtr circle(new viz::ElementArrowCircle);
- circle->id = "circle";
- circle->action = viz::Element_UPDATE;
-
- float modTime = std::fmod(timeInSeconds, 2.0 * M_PI);
- circle->completion = std::sin(modTime);
+ viz::LayerUpdate layer;
+ layer.component = getName();
+ layer.name = "Example";
+ {
+ viz::ElementArrowCirclePtr circle(new viz::ElementArrowCircle);
+ circle->id = "circle";
+ circle->action = viz::Element_UPDATE;
- circle->radius = 100.0f;
- circle->width = 10.0f;
+ float modTime = std::fmod(timeInSeconds, 2.0 * M_PI);
+ circle->completion = std::sin(modTime);
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = +300.0f;
- circle->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
- circle->color = armarx::viz::Color{200, 255, 0, 255};
+ circle->radius = 100.0f;
+ circle->width = 10.0f;
- layer.elements.push_back(circle);
- }
- {
- viz::ElementPolygonPtr poly(new viz::ElementPolygon);
- poly->id = "poly";
- poly->action = viz::Element_UPDATE;
-
- poly->color = armarx::viz::Color{128, 0, 128, 255};
- poly->lineColor = {255, 0, 0, 255};
- poly->lineWidth = 10.0f;
-
- float t = 1.0f + std::sin(timeInSeconds);
- float offset = 50.0f * t;
- poly->points.push_back(armarx::Vector3f{-200.0f - offset, -200.0f - offset, 0.0f});
- poly->points.push_back(armarx::Vector3f{-200.0f, +200.0f, 0.0f});
- poly->points.push_back(armarx::Vector3f{+200.0f + offset, +200.0f + offset, 0.0f});
- poly->points.push_back(armarx::Vector3f{+200.0f, -200.0f, 0.0f});
-
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = +1000.0f;
- poly->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
-
- layer.elements.push_back(poly);
- }
- {
- viz::ElementPolygonPtr poly(new viz::ElementPolygon);
- poly->id = "poly2";
- poly->action = viz::Element_UPDATE;
-
- poly->color = armarx::viz::Color{128, 255, 128, 0};
- poly->lineColor = {255, 255, 0, 0};
- poly->lineWidth = 0.0f;
-
- float t = 1.0f + std::sin(timeInSeconds);
- float offset = 20.0f * t;
- poly->points.push_back(armarx::Vector3f{-100.0f - offset, -100.0f - offset, 0.0f});
- poly->points.push_back(armarx::Vector3f{-100.0f, +100.0f, 0.0f});
- poly->points.push_back(armarx::Vector3f{+100.0f + offset, +100.0f + offset, 0.0f});
- poly->points.push_back(armarx::Vector3f{+100.0f, -100.0f, 0.0f});
-
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = +1500.0f;
- poly->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
-
- layer.elements.push_back(poly);
- }
- {
- viz::ElementMeshPtr mesh(new viz::ElementMesh);
- mesh->id = "mesh";
- mesh->action = viz::Element_UPDATE;
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.z() = +300.0f;
+ circle->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
+ circle->color = armarx::viz::Color{200, 255, 0, 255};
- armarx::Vector3f vertices[] =
+ layer.elements.push_back(circle);
+ }
{
- {-100.0f, -100.0f, 0.0f},
- {-100.0f, +100.0f, 0.0f},
- {+100.0f, +100.0f, 0.0f},
- {+100.0f, +100.0f, 200.0f},
- };
- armarx::viz::Color colors[] =
+ viz::ElementPolygonPtr poly(new viz::ElementPolygon);
+ poly->id = "poly";
+ poly->action = viz::Element_UPDATE;
+
+ poly->color = armarx::viz::Color{128, 0, 128, 255};
+ poly->lineColor = {255, 0, 0, 255};
+ poly->lineWidth = 10.0f;
+
+ float t = 1.0f + std::sin(timeInSeconds);
+ float offset = 50.0f * t;
+ poly->points.push_back(armarx::Vector3f{-200.0f - offset, -200.0f - offset, 0.0f});
+ poly->points.push_back(armarx::Vector3f{-200.0f, +200.0f, 0.0f});
+ poly->points.push_back(armarx::Vector3f{+200.0f + offset, +200.0f + offset, 0.0f});
+ poly->points.push_back(armarx::Vector3f{+200.0f, -200.0f, 0.0f});
+
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.z() = +1000.0f;
+ poly->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
+
+ layer.elements.push_back(poly);
+ }
{
- {255, 255, 0, 0},
- {255, 0, 255, 0},
- {255, 0, 0, 255},
- };
- armarx::viz::Face faces[] =
+ viz::ElementPolygonPtr poly(new viz::ElementPolygon);
+ poly->id = "poly2";
+ poly->action = viz::Element_UPDATE;
+
+ poly->color = armarx::viz::Color{128, 255, 128, 0};
+ poly->lineColor = {255, 255, 0, 0};
+ poly->lineWidth = 0.0f;
+
+ float t = 1.0f + std::sin(timeInSeconds);
+ float offset = 20.0f * t;
+ poly->points.push_back(armarx::Vector3f{-100.0f - offset, -100.0f - offset, 0.0f});
+ poly->points.push_back(armarx::Vector3f{-100.0f, +100.0f, 0.0f});
+ poly->points.push_back(armarx::Vector3f{+100.0f + offset, +100.0f + offset, 0.0f});
+ poly->points.push_back(armarx::Vector3f{+100.0f, -100.0f, 0.0f});
+
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.z() = +1500.0f;
+ poly->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
+
+ layer.elements.push_back(poly);
+ }
{
+ viz::ElementMeshPtr mesh(new viz::ElementMesh);
+ mesh->id = "mesh";
+ mesh->action = viz::Element_UPDATE;
+
+ armarx::Vector3f vertices[] =
{
- 0, 1, 2,
- 0, 1, 2,
- },
+ {-100.0f, -100.0f, 0.0f},
+ {-100.0f, +100.0f, 0.0f},
+ {+100.0f, +100.0f, 0.0f},
+ {+100.0f, +100.0f, 200.0f},
+ };
+ armarx::viz::Color colors[] =
{
- 1, 2, 3,
- 0, 1, 2,
- },
- };
- std::size_t verticesSize = sizeof(vertices) / sizeof(vertices[0]);
- mesh->vertices.assign(vertices, vertices + verticesSize);
- std::size_t colorsSize = sizeof(colors) / sizeof(colors[0]);
- mesh->colors.assign(colors, colors + colorsSize);
- std::size_t facesSize = sizeof(faces) / sizeof(faces[0]);
- mesh->faces.assign(faces, faces + facesSize);
-
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.z() = +1000.0f;
- pos.x() = -500.0f;
- mesh->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
-
- layer.elements.push_back(mesh);
- }
- {
- viz::ElementRobotPtr robot(new viz::ElementRobot);
- robot->id = "robot";
- robot->action = viz::Element_UPDATE;
+ {255, 255, 0, 0},
+ {255, 0, 255, 0},
+ {255, 0, 0, 255},
+ };
+ armarx::viz::Face faces[] =
+ {
+ {
+ 0, 1, 2,
+ 0, 1, 2,
+ },
+ {
+ 1, 2, 3,
+ 0, 1, 2,
+ },
+ };
+ std::size_t verticesSize = sizeof(vertices) / sizeof(vertices[0]);
+ mesh->vertices.assign(vertices, vertices + verticesSize);
+ std::size_t colorsSize = sizeof(colors) / sizeof(colors[0]);
+ mesh->colors.assign(colors, colors + colorsSize);
+ std::size_t facesSize = sizeof(faces) / sizeof(faces[0]);
+ mesh->faces.assign(faces, faces + facesSize);
+
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.z() = +1000.0f;
+ pos.x() = -500.0f;
+ mesh->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
+
+ layer.elements.push_back(mesh);
+ }
+ {
+ viz::ElementRobotPtr robot(new viz::ElementRobot);
+ robot->id = "robot";
+ robot->action = viz::Element_UPDATE;
- robot->project = "Armar6RT";
- robot->filename = "Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml";
+ robot->project = "Armar6RT";
+ robot->filename = "Armar6RT/robotmodel/Armar6-SH/Armar6-SH.xml";
- Eigen::Vector3f pos = Eigen::Vector3f::Zero();
- pos.x() = 500.0f;
- robot->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
+ Eigen::Vector3f pos = Eigen::Vector3f::Zero();
+ pos.x() = 500.0f;
+ robot->pose = makeGlobalPose(pos, Eigen::Quaternionf::Identity());
- float value = 0.5f * (1.0f + std::sin(timeInSeconds));
- robot->jointValues["ArmR2_Sho1"] = value;
- robot->jointValues["ArmR3_Sho2"] = value;
+ float value = 0.5f * (1.0f + std::sin(timeInSeconds));
+ robot->jointValues["ArmR2_Sho1"] = value;
+ robot->jointValues["ArmR3_Sho2"] = value;
- //robot->drawStyle = viz::ModelDrawStyle_FULL_MODEL;
- robot->color = armarx::viz::Color{255, 128, 128, 255};
+ //robot->drawStyle = viz::ModelDrawStyle_FULL_MODEL;
+ robot->color = armarx::viz::Color{255, 128, 128, 255};
- layer.elements.push_back(robot);
+ layer.elements.push_back(robot);
+ }
+ topic->updateLayers({layer});
}
- topic->updateLayers({layer});
c.waitForCycleDuration();
}
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index a8716ed3fb2c1f4c7d1da21372c4556027c393d1..df517e280a2e535a9617678d3e1be008739ce470 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -78,18 +78,21 @@ void armarx::ArVizWidgetController::onDisconnectComponent()
void ArVizWidgetController::layerTreeChanged(QTreeWidgetItem* item, int /*column*/)
{
// Iterate over all items and activate/deactivate layers accordingly
- 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();
Qt::CheckState componentCheck = componentItem->checkState(0);
+ int layerCount = componentItem->childCount();
if (componentItem == item)
{
// The parent was selected or deselected, so all children should be set accordingly
- for (QTreeWidgetItemIterator layerIter(componentItem); *layerIter; ++layerIter)
+ ARMARX_VERBOSE << "Setting all children of " << component << " to " << (componentCheck == Qt::Checked);
+ for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex)
{
- QTreeWidgetItem* layerItem = *layerIter;
+ QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
if (layerItem->checkState(0) != componentCheck)
{
layerItem->setCheckState(0, componentCheck);
@@ -98,16 +101,15 @@ void ArVizWidgetController::layerTreeChanged(QTreeWidgetItem* item, int /*column
return;
}
- for (QTreeWidgetItemIterator layerIter(componentItem); *layerIter; ++layerIter)
+ for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex)
{
- QTreeWidgetItem* layerItem = *layerIter;
+ QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
std::string layer = layerItem->text(0).toStdString();
bool layerVisible = (layerItem->checkState(0) == Qt::Checked);
-
- bool visible = layerVisible;
+ ARMARX_VERBOSE << "Layer: " << layer << ", Visible: " << layerVisible;
viz::CoinLayerID layerID(component, layer);
- visualizer.showLayer(layerID, visible);
+ visualizer.showLayer(layerID, layerVisible);
}
}
}
@@ -115,35 +117,67 @@ void ArVizWidgetController::layerTreeChanged(QTreeWidgetItem* item, int /*column
void ArVizWidgetController::layersChanged(std::vector<viz::CoinLayerID> const& layers)
{
QTreeWidget* tree = widget.layerTree;
- QTreeWidgetItem* currentItem = nullptr;
- // TODO: We need to keep the state of the old tree items
- // HACK: Just remove all the children before adding them again
- tree->clear();
+ std::map<std::string, QTreeWidgetItem*> currentComponents;
+ std::map<viz::CoinLayerID, QTreeWidgetItem*> currentLayers;
+ for (QTreeWidgetItemIterator compIter(widget.layerTree); *compIter; ++compIter)
+ {
+ QTreeWidgetItem* componentItem = *compIter;
+ std::string component = componentItem->text(0).toStdString();
+ currentComponents.emplace(component, componentItem);
+
+ for (QTreeWidgetItemIterator layerIter(componentItem); *layerIter; ++layerIter)
+ {
+ QTreeWidgetItem* layerItem = *layerIter;
+ std::string layer = layerItem->text(0).toStdString();
+
+ viz::CoinLayerID layerID(component, layer);
+ currentLayers.emplace(layerID, layerItem);
+ }
+ }
+ // We need to determine which layers are new and where to append them
+ QTreeWidgetItem* currentItem = nullptr;
std::string currentComponent;
for (auto& entry : layers)
{
std::string const& component = entry.first;
if (component != currentComponent)
{
- // Create a new item
- currentItem = new QTreeWidgetItem(tree);
- currentItem->setText(0, QString::fromStdString(component));
- // A new item is visible by default
- currentItem->setCheckState(0, Qt::Checked);
+ auto iter = currentComponents.find(component);
+ if (iter == currentComponents.end())
+ {
+ // Create a new item
+ currentItem = new QTreeWidgetItem(tree);
+ currentItem->setText(0, QString::fromStdString(component));
+ // A new item is visible by default
+ currentItem->setCheckState(0, Qt::Checked);
+ }
+ else
+ {
+ // Item exists already
+ currentItem = iter->second;
+ }
currentComponent = component;
}
- std::string const& layer = entry.second;
- QTreeWidgetItem* layerItem = new QTreeWidgetItem;
- layerItem->setText(0, QString::fromStdString(layer));
- // A new item is visible by default
- // TODO: Take into account the old state!
- layerItem->setCheckState(0, Qt::Checked);
+ auto iter = currentLayers.find(entry);
+ if (iter == currentLayers.end())
+ {
+ // Create a new item
+ std::string const& layer = entry.second;
+ QTreeWidgetItem* layerItem = new QTreeWidgetItem;
+ layerItem->setText(0, QString::fromStdString(layer));
+ // A new item is visible by default
+ layerItem->setCheckState(0, Qt::Checked);
- currentItem->addChild(layerItem);
+ currentItem->addChild(layerItem);
+ }
+ else
+ {
+ // Item exists already == nothing to be done
+ }
}
}