From 3243170e3b5afb39c9f67c975d61267a2a0f5a0b Mon Sep 17 00:00:00 2001
From: Peter Kaiser <peter.kaiser@kit.edu>
Date: Fri, 10 Jun 2016 11:04:33 +0200
Subject: [PATCH] DebugDrawer: Extended interface for manual element
selection/deselection
---
.../DebugDrawer/DebugDrawerComponent.cpp | 155 ++++++++++++------
.../DebugDrawer/DebugDrawerComponent.h | 3 +
.../visualization/DebugDrawerInterface.ice | 18 +-
3 files changed, 128 insertions(+), 48 deletions(-)
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 1acd95e33..919276998 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -63,7 +63,9 @@
using namespace VirtualRobot;
-#define SELECTION_NAME_PREFIX ("selection_" + boost::lexical_cast<std::string>(this) + "_")
+#define SELECTION_NAME_PREFIX ("selection_" + boost::lexical_cast<std::string>(this))
+#define SELECTION_NAME_SPLITTER "____"
+#define SELECTION_NAME(layerName, elementName) (SELECTION_NAME_PREFIX + "_" + layerName + SELECTION_NAME_SPLITTER + elementName).c_str()
namespace armarx
{
@@ -103,6 +105,7 @@ namespace armarx
selectionNode = new SoSelection;
selectionNode->policy = SoSelection::SHIFT;
+ selectionNode->ref();
coinVisu->addChild(selectionNode);
layerMainNode = new SoSeparator;
@@ -128,7 +131,7 @@ namespace armarx
{
selectableLayers.clear();
}
- else
+ else if (layers.find(layerName) != layers.end())
{
selectableLayers.erase(layerName);
}
@@ -143,12 +146,62 @@ namespace armarx
{
selectionNode->deselectAll();
}
- else
+ else if (layers.find(layerName) != layers.end())
{
selectionNode->deselect(layers.at(layerName).mainNode);
}
}
+ void DebugDrawerComponent::select(const std::string& layerName, const std::string& elementName, const Ice::Current&)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+ ScopedRecursiveLockPtr l2(new ScopedRecursiveLock(selectionMutex));
+
+ removeSelectionCallbacks();
+
+ ARMARX_DEBUG << "Selecting element '" << elementName << "' on layer '" << layerName << "' (internal name: " << SELECTION_NAME(layerName, elementName) << ")";
+
+ SoNode* n = SoSelection::getByName(SELECTION_NAME(layerName, elementName));
+ if (n)
+ {
+ selectionNode->select(n);
+ }
+ else
+ {
+ ARMARX_WARNING << "Element to select not found";
+ }
+
+ // Force visualization update
+ selectionNode->touch();
+
+ installSelectionCallbacks();
+ }
+
+ void DebugDrawerComponent::deselect(const std::string& layerName, const std::string& elementName, const Ice::Current&)
+ {
+ ScopedRecursiveLockPtr l = getScopedVisuLock();
+ ScopedRecursiveLockPtr l2(new ScopedRecursiveLock(selectionMutex));
+
+ removeSelectionCallbacks();
+
+ ARMARX_DEBUG << "Deselecting element '" << elementName << "' on layer '" << layerName << "' (internal name: " << SELECTION_NAME(layerName, elementName) << ")";
+
+ SoNode* n = SoSelection::getByName(SELECTION_NAME(layerName, elementName));
+ if (n)
+ {
+ selectionNode->deselect(n);
+ }
+ else
+ {
+ ARMARX_WARNING << "Element to deselect not found";
+ }
+
+ // Force visualization update
+ selectionNode->touch();
+
+ installSelectionCallbacks();
+ }
+
void DebugDrawerComponent::selectionCallback()
{
ScopedRecursiveLockPtr l(new ScopedRecursiveLock(*topicMutex));
@@ -183,9 +236,9 @@ namespace armarx
removeSelectionCallbacks();
selectionNode->deselectAll();
- for (auto & e : selectedElements)
+ for (auto& e : selectedElements)
{
- SoNode* n = SoSelection::getByName((SELECTION_NAME_PREFIX + e).c_str());
+ SoNode* n = SoSelection::getByName(SELECTION_NAME(e.layerName, e.elementName));
if (n)
{
selectionNode->select(n);
@@ -211,17 +264,24 @@ namespace armarx
std::string name = path->getTail()->getName().getString();
if (name.length() > 0 && name.find(SELECTION_NAME_PREFIX) == 0)
{
- name = name.substr(SELECTION_NAME_PREFIX.length());
+ int index = name.rfind(SELECTION_NAME_SPLITTER);
+ name = name.substr(index + strlen(SELECTION_NAME_SPLITTER));
// Check if selected element is 'selectable'
- for (auto & layer : selectableLayers)
+ for (auto& l : layers)
{
+ std::string layer = l.first;
if (layers[layer].addedBoxVisualizations.find(name) != layers[layer].addedBoxVisualizations.end()
|| layers[layer].addedTextVisualizations.find(name) != layers[layer].addedTextVisualizations.end()
|| layers[layer].addedSphereVisualizations.find(name) != layers[layer].addedSphereVisualizations.end()
- || layers[layer].addedCylinderVisualizations.find(name) != layers[layer].addedCylinderVisualizations.end())
+ || layers[layer].addedCylinderVisualizations.find(name) != layers[layer].addedCylinderVisualizations.end()
+ || layers[layer].addedPolygonVisualizations.find(name) != layers[layer].addedPolygonVisualizations.end())
{
- selectedElements.push_back(name);
+ DebugDrawerSelectionElement e;
+ e.layerName = layer;
+ e.elementName = name;
+
+ selectedElements.push_back(e);
break;
}
}
@@ -352,6 +412,7 @@ namespace armarx
ScopedRecursiveLockPtr l = getScopedVisuLock();
coinVisu->removeAllChildren();
layerMainNode->unref();
+ selectionNode->unref();
coinVisu->unref();
}
}
@@ -549,7 +610,7 @@ namespace armarx
newS->addChild(material);
SoCube* cube = new SoCube;
- cube->setName((SELECTION_NAME_PREFIX + d.name).c_str());
+ cube->setName(SELECTION_NAME(d.layerName, d.name));
cube->width = d.width;
cube->height = d.height;
cube->depth = d.depth;
@@ -594,7 +655,7 @@ namespace armarx
ann->addChild(font);
SoText2* te = new SoText2;
- te->setName((SELECTION_NAME_PREFIX + d.name).c_str());
+ te->setName(SELECTION_NAME(d.layerName, d.name));
te->string = d.text.c_str();
te->justification = SoText2::CENTER;
ann->addChild(te);
@@ -631,7 +692,7 @@ namespace armarx
sep->addChild(tr);
SoSphere* sphere = new SoSphere;
- sphere->setName((SELECTION_NAME_PREFIX + d.name).c_str());
+ sphere->setName(SELECTION_NAME(d.layerName, d.name));
sphere->radius = d.radius;
sep->addChild(sphere);
@@ -667,7 +728,7 @@ namespace armarx
sep->addChild(tr);
SoCylinder* cylinder = new SoCylinder;
- cylinder->setName((SELECTION_NAME_PREFIX + d.name).c_str());
+ cylinder->setName(SELECTION_NAME(d.layerName, d.name));
cylinder->height = d.length;
cylinder->radius = d.radius;
sep->addChild(cylinder);
@@ -739,11 +800,11 @@ namespace armarx
}
SoSeparator* sep = VirtualRobot::CoinVisualizationFactory::CreatePolygonVisualization(d.points, d.colorInner, d.colorBorder, d.lineWidth);
+ sep->setName(SELECTION_NAME(d.layerName, d.name));
layer.addedPolygonVisualizations[d.name] = sep;
layer.mainNode->addChild(sep);
}
-
void DebugDrawerComponent::drawTriMesh(const DebugDrawerComponent::TriMeshData& d)
{
ScopedRecursiveLockPtr l = getScopedVisuLock();
@@ -1835,7 +1896,7 @@ namespace armarx
d.layerName = layerName;
d.name = robotName;
- for (auto & it : configuration)
+ for (auto& it : configuration)
{
d.configuration[it.first] = it.second;
}
@@ -1882,7 +1943,7 @@ namespace armarx
void DebugDrawerComponent::clearAll(const Ice::Current&)
{
- for (auto & i : layers)
+ for (auto& i : layers)
{
clearLayer(i.first);
}
@@ -1925,72 +1986,72 @@ namespace armarx
Layer& layer = layers.at(layerName);
- for (const auto & i : layer.addedCoordVisualizations)
+ for (const auto& i : layer.addedCoordVisualizations)
{
removePoseVisu(layerName, i.first);
}
- for (const auto & i : layer.addedLineVisualizations)
+ for (const auto& i : layer.addedLineVisualizations)
{
removeLineVisu(layerName, i.first);
}
- for (const auto & i : layer.addedLineSetVisualizations)
+ for (const auto& i : layer.addedLineSetVisualizations)
{
removeLineSetVisu(layerName, i.first);
}
- for (const auto & i : layer.addedBoxVisualizations)
+ for (const auto& i : layer.addedBoxVisualizations)
{
removeBoxVisu(layerName, i.first);
}
- for (const auto & i : layer.addedTextVisualizations)
+ for (const auto& i : layer.addedTextVisualizations)
{
removeTextVisu(layerName, i.first);
}
- for (const auto & i : layer.addedSphereVisualizations)
+ for (const auto& i : layer.addedSphereVisualizations)
{
removeSphereVisu(layerName, i.first);
}
- for (const auto & i : layer.addedCylinderVisualizations)
+ for (const auto& i : layer.addedCylinderVisualizations)
{
removeCylinderVisu(layerName, i.first);
}
- for (const auto & i : layer.addedPointCloudVisualizations)
+ for (const auto& i : layer.addedPointCloudVisualizations)
{
removePointCloudVisu(layerName, i.first);
}
- for (const auto & i : layer.addedPolygonVisualizations)
+ for (const auto& i : layer.addedPolygonVisualizations)
{
removePolygonVisu(layerName, i.first);
}
- for (const auto & i : layer.addedTriMeshVisualizations)
+ for (const auto& i : layer.addedTriMeshVisualizations)
{
removeTriMeshVisu(layerName, i.first);
}
- for (const auto & i : layer.added24BitColoredPointCloudVisualizations)
+ for (const auto& i : layer.added24BitColoredPointCloudVisualizations)
{
remove24BitColoredPointCloudVisu(layerName, i.first);
}
- for (const auto & i : layer.addedArrowVisualizations)
+ for (const auto& i : layer.addedArrowVisualizations)
{
removeArrowVisu(layerName, i.first);
}
- for (const auto & i : layer.addedRobotVisualizations)
+ for (const auto& i : layer.addedRobotVisualizations)
{
removeRobotVisu(layerName, i.first);
}
- for (const auto & i : layer.addedCustomVisualizations)
+ for (const auto& i : layer.addedCustomVisualizations)
{
removeCustomVisu(layerName, i.first);
}
@@ -2063,74 +2124,74 @@ namespace armarx
// check for clear&remove
//(updates only contain elements to add for each layer after the last clear for this layer was executed)
//this prevents a sequence add,clear,add to result in an empty layer
- for (const auto & layer : accumulatedUpdateData.clearLayers)
+ for (const auto& layer : accumulatedUpdateData.clearLayers)
{
clearLayerQt(layer);
}
accumulatedUpdateData.clearLayers.clear();
- for (const auto & layer : accumulatedUpdateData.removeLayers)
+ for (const auto& layer : accumulatedUpdateData.removeLayers)
{
removeLayerQt(layer);
}
accumulatedUpdateData.removeLayers.clear();
//add elements
- for (auto & e : accumulatedUpdateData.coord)
+ for (auto& e : accumulatedUpdateData.coord)
{
drawCoordSystem(e.second);
}
accumulatedUpdateData.coord.clear();
- for (auto & e : accumulatedUpdateData.box)
+ for (auto& e : accumulatedUpdateData.box)
{
drawBox(e.second);
}
accumulatedUpdateData.box.clear();
- for (auto & e : accumulatedUpdateData.line)
+ for (auto& e : accumulatedUpdateData.line)
{
drawLine(e.second);
}
accumulatedUpdateData.line.clear();
- for (auto & e : accumulatedUpdateData.lineSet)
+ for (auto& e : accumulatedUpdateData.lineSet)
{
drawLineSet(e.second);
}
accumulatedUpdateData.lineSet.clear();
- for (auto & e : accumulatedUpdateData.sphere)
+ for (auto& e : accumulatedUpdateData.sphere)
{
drawSphere(e.second);
}
accumulatedUpdateData.sphere.clear();
- for (auto & e : accumulatedUpdateData.cylinder)
+ for (auto& e : accumulatedUpdateData.cylinder)
{
drawCylinder(e.second);
}
accumulatedUpdateData.cylinder.clear();
- for (auto & e : accumulatedUpdateData.text)
+ for (auto& e : accumulatedUpdateData.text)
{
drawText(e.second);
}
accumulatedUpdateData.text.clear();
- for (auto & e : accumulatedUpdateData.pointcloud)
+ for (auto& e : accumulatedUpdateData.pointcloud)
{
drawPointCloud(e.second);
}
accumulatedUpdateData.pointcloud.clear();
- for (auto & e : accumulatedUpdateData.polygons)
+ for (auto& e : accumulatedUpdateData.polygons)
{
drawPolygon(e.second);
}
accumulatedUpdateData.polygons.clear();
- for (auto & e : accumulatedUpdateData.arrows)
+ for (auto& e : accumulatedUpdateData.arrows)
{
drawArrow(e.second);
}
@@ -2142,7 +2203,7 @@ namespace armarx
}
accumulatedUpdateData.triMeshes.clear();
- for (auto & e : accumulatedUpdateData.robots)
+ for (auto& e : accumulatedUpdateData.robots)
{
ARMARX_DEBUG << "update visu / drawRobot for robot " << e.first;
ensureExistingRobotNodes(e.second);
@@ -2151,13 +2212,13 @@ namespace armarx
}
accumulatedUpdateData.robots.clear();
- for (auto & e : accumulatedUpdateData.coloredpointcloud)
+ for (auto& e : accumulatedUpdateData.coloredpointcloud)
{
drawColoredPointCloud(e.second);
}
accumulatedUpdateData.coloredpointcloud.clear();
- for (auto & e : accumulatedUpdateData.colored24Bitpointcloud)
+ for (auto& e : accumulatedUpdateData.colored24Bitpointcloud)
{
draw24BitColoredPointCloud(e.second);
}
@@ -2262,7 +2323,7 @@ namespace armarx
ScopedRecursiveLockPtr l = getScopedVisuLock();
Ice::StringSeq seq {};
- for (const auto & layer : layers)
+ for (const auto& layer : layers)
{
seq.push_back(layer.first);
}
@@ -2275,7 +2336,7 @@ namespace armarx
::armarx::LayerInformationSequence seq {};
ScopedRecursiveLockPtr l = getScopedVisuLock();
- for (const auto & layer : layers)
+ for (const auto& layer : layers)
{
int count = layer.second.addedCoordVisualizations.size() +
layer.second.addedLineVisualizations.size() +
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index 52706905d..5394c8a6c 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -255,6 +255,9 @@ namespace armarx
virtual void enableSelections(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
virtual void disableSelections(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
virtual void clearSelections(const std::string& layerName, const ::Ice::Current& = ::Ice::Current());
+ void select(const std::string& layerName, const std::string& elementName, const ::Ice::Current& = ::Ice::Current());
+ void deselect(const std::string& layerName, const std::string& elementName, const ::Ice::Current& = ::Ice::Current());
+
virtual DebugDrawerSelectionList getSelections(const ::Ice::Current& = ::Ice::Current());
void selectionCallback();
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index 249663304..790b47cd4 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -186,7 +186,13 @@ module armarx
enum DrawStyle { FullModel, CollisionModel };
- sequence<string> DebugDrawerSelectionList;
+ struct DebugDrawerSelectionElement
+ {
+ string layerName;
+ string elementName;
+ };
+
+ sequence<DebugDrawerSelectionElement> DebugDrawerSelectionList;
/*!
* \brief A layered drawing interface.
@@ -372,6 +378,16 @@ module armarx
*/
void clearSelections(string layerName);
+ /*!
+ * \brief Select element in visualization
+ */
+ void select(string layerName, string elementName);
+
+ /*!
+ * \brief Deselect element in visualization
+ */
+ void deselect(string layerName, string elementName);
+
/*!
* \brief Return currently selected elements
*/
--
GitLab