diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index de97a1c73959f5174f8c0699e1e017239fd64776..d491dec54e1471733d21f38c07862e929e6537fa 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -70,7 +70,10 @@ void DebugDrawerComponent::onExitComponent()
{
ARMARX_DEBUG << "onExitComponent";
- layers.clear();
+ for(auto& layer:layers)
+ {
+ removeLayer(layer.first);
+ }
coinVisu->unref();
}
@@ -79,7 +82,7 @@ void DebugDrawerComponent::drawCoordSystem(const std::string &layerName, const s
ScopedRecursiveLockPtr l = getScopedLock();
ARMARX_DEBUG << "drawing coord system";
- auto layer=requestLayer(layerName);
+ auto& layer=requestLayer(layerName);
removeCoordSystem(layerName, name);
@@ -101,10 +104,10 @@ void DebugDrawerComponent::drawCoordSystem(const std::string &layerName, const s
newS->addChild(newM);
std::string n = name;
newS->addChild(CoinVisualizationFactory::CreateCoordSystemVisualization(scale,&n));
- layer->addedCoordVisualizations[name] = newS;
- layer->mainNode->addChild(newS);
+ layer.addedCoordVisualizations[name] = newS;
+ layer.mainNode->addChild(newS);
//}
- SoSeparator *s = layer->addedCoordVisualizations[name];
+ SoSeparator *s = layer.addedCoordVisualizations[name];
SoMatrixTransform* m = (SoMatrixTransform*)(s->getChild(0));
SbMatrix mNew = CoinVisualizationFactory::getMatrixTransform(globalPose)->matrix.getValue();
m->matrix.setValue(mNew);
@@ -117,7 +120,7 @@ void DebugDrawerComponent::drawLine(const std::string &layerName, const std::str
ScopedRecursiveLockPtr l = getScopedLock();
ARMARX_DEBUG << "drawLine" << flush;
- auto layer=requestLayer(layerName);
+ auto& layer=requestLayer(layerName);
removeLine(layerName,name);
@@ -127,8 +130,8 @@ void DebugDrawerComponent::drawLine(const std::string &layerName, const std::str
Eigen::Matrix4f lp2 = Eigen::Matrix4f::Identity();
lp2(0,3) = p2.x(); lp2(1,3) = p2.y(); lp2(2,3) = p2.z();
newS->addChild(CoinVisualizationFactory::createCoinLine(lp1, lp2, scale, color.r, color.g, color.b));
- layer->addedLineVisualizations[name] = newS;
- layer->mainNode->addChild(newS);
+ layer.addedLineVisualizations[name] = newS;
+ layer.mainNode->addChild(newS);
}
void DebugDrawerComponent::drawBox(const std::string &layerName, const std::string &name, Eigen::Matrix4f &globalPose, float width, float height, float depth, VisualizationFactory::Color &color)
@@ -136,7 +139,7 @@ void DebugDrawerComponent::drawBox(const std::string &layerName, const std::stri
ScopedRecursiveLockPtr l = getScopedLock();
ARMARX_DEBUG << "drawBox";
- auto layer=requestLayer(layerName);
+ auto& layer=requestLayer(layerName);
removeBox(layerName,name);
@@ -155,8 +158,8 @@ void DebugDrawerComponent::drawBox(const std::string &layerName, const std::stri
cube->depth = depth;
newS->addChild(cube);
- layer->addedBoxVisualizations[name] = newS;
- layer->mainNode->addChild(newS);
+ layer.addedBoxVisualizations[name] = newS;
+ layer.mainNode->addChild(newS);
}
void DebugDrawerComponent::drawText(const std::string &layerName, const std::string &name, const std::string &text, const Eigen::Vector3f &position, const VisualizationFactory::Color &color, int size)
@@ -164,7 +167,7 @@ void DebugDrawerComponent::drawText(const std::string &layerName, const std::str
ScopedRecursiveLockPtr l = getScopedLock();
ARMARX_DEBUG << "drawText";
- auto layer=requestLayer(layerName);
+ auto& layer=requestLayer(layerName);
removeText(layerName,name);
@@ -192,8 +195,8 @@ void DebugDrawerComponent::drawText(const std::string &layerName, const std::str
te->justification = SoText2::CENTER;
ann->addChild(te);
- layer->addedTextVisualizations[name] = sep;
- layer->mainNode->addChild(sep);
+ layer.addedTextVisualizations[name] = sep;
+ layer.mainNode->addChild(sep);
}
void DebugDrawerComponent::drawSphere(const std::string& layerName, const std::string &name, const Eigen::Vector3f &position, const VisualizationFactory::Color &color, float radius)
@@ -201,7 +204,7 @@ void DebugDrawerComponent::drawSphere(const std::string& layerName, const std::s
ScopedRecursiveLockPtr l = getScopedLock();
ARMARX_DEBUG << "drawSphere";
- auto layer=requestLayer(layerName);
+ auto& layer=requestLayer(layerName);
removeSphere(layerName,name);
@@ -220,8 +223,8 @@ void DebugDrawerComponent::drawSphere(const std::string& layerName, const std::s
sphere->radius = radius;
sep->addChild(sphere);
- layer->addedSphereVisualizations[name] = sep;
- layer->mainNode->addChild(sep);
+ layer.addedSphereVisualizations[name] = sep;
+ layer.mainNode->addChild(sep);
}
@@ -233,13 +236,13 @@ void DebugDrawerComponent::removeLine(const std::string& layerName, const std::s
return;
}
ScopedRecursiveLockPtr l = getScopedLock();
- auto layer=layers.at(layerName);
- if (layer->addedLineVisualizations.find(name) == layer->addedLineVisualizations.end())
+ auto& layer=layers.at(layerName);
+ if (layer.addedLineVisualizations.find(name) == layer.addedLineVisualizations.end())
{
return;
}
- layer->mainNode->removeChild(layer->addedLineVisualizations[name]);
- layer->addedLineVisualizations.erase(name);
+ layer.mainNode->removeChild(layer.addedLineVisualizations[name]);
+ layer.addedLineVisualizations.erase(name);
}
void DebugDrawerComponent::removeBox(const std::string& layerName, const std::string &name)
@@ -249,14 +252,14 @@ void DebugDrawerComponent::removeBox(const std::string& layerName, const std::st
return;
}
ScopedRecursiveLockPtr l = getScopedLock();
- auto layer=layers.at(layerName);
- if(layer->addedBoxVisualizations.find(name) == layer->addedBoxVisualizations.end())
+ auto& layer=layers.at(layerName);
+ if(layer.addedBoxVisualizations.find(name) == layer.addedBoxVisualizations.end())
{
return;
}
- layer->mainNode->removeChild(layer->addedBoxVisualizations[name]);
- layer->addedBoxVisualizations.erase(name);
+ layer.mainNode->removeChild(layer.addedBoxVisualizations[name]);
+ layer.addedBoxVisualizations.erase(name);
}
void DebugDrawerComponent::removeText(const std::string &layerName, const std::string &name)
@@ -266,14 +269,14 @@ void DebugDrawerComponent::removeText(const std::string &layerName, const std::s
return;
}
ScopedRecursiveLockPtr l = getScopedLock();
- auto layer=layers.at(layerName);
- if(layer->addedTextVisualizations.find(name) == layer->addedTextVisualizations.end())
+ auto& layer=layers.at(layerName);
+ if(layer.addedTextVisualizations.find(name) == layer.addedTextVisualizations.end())
{
return;
}
- layer->mainNode->removeChild(layer->addedTextVisualizations[name]);
- layer->addedTextVisualizations.erase(name);
+ layer.mainNode->removeChild(layer.addedTextVisualizations[name]);
+ layer.addedTextVisualizations.erase(name);
}
void DebugDrawerComponent::removeSphere(const std::string &layerName, const std::string &name)
@@ -283,14 +286,14 @@ void DebugDrawerComponent::removeSphere(const std::string &layerName, const std:
return;
}
ScopedRecursiveLockPtr l = getScopedLock();
- auto layer=layers.at(layerName);
- if(layer->addedSphereVisualizations.find(name) == layer->addedSphereVisualizations.end())
+ auto& layer=layers.at(layerName);
+ if(layer.addedSphereVisualizations.find(name) == layer.addedSphereVisualizations.end())
{
return;
}
- layer->mainNode->removeChild(layer->addedSphereVisualizations[name]);
- layer->addedSphereVisualizations.erase(name);
+ layer.mainNode->removeChild(layer.addedSphereVisualizations[name]);
+ layer.addedSphereVisualizations.erase(name);
}
void DebugDrawerComponent::removeCoordSystem(const std::string& layerName, const std::string &name)
@@ -300,14 +303,14 @@ void DebugDrawerComponent::removeCoordSystem(const std::string& layerName, const
return;
}
ScopedRecursiveLockPtr l = getScopedLock();
- auto layer=layers.at(layerName);
- if(layer->addedCoordVisualizations.find(name) == layer->addedCoordVisualizations.end())
+ auto& layer=layers.at(layerName);
+ if(layer.addedCoordVisualizations.find(name) == layer.addedCoordVisualizations.end())
{
return;
}
- layer->mainNode->removeChild(layer->addedCoordVisualizations[name]);
- layer->addedCoordVisualizations.erase(name);
+ layer.mainNode->removeChild(layer.addedCoordVisualizations[name]);
+ layer.addedCoordVisualizations.erase(name);
}
void DebugDrawerComponent::setScaledPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current&)
@@ -441,13 +444,19 @@ void DebugDrawerComponent::clearLayer(const std::string &layerName, const Ice::C
return;
}
ARMARX_VERBOSE << "Clearing layer " << layerName;
- layers.at(layerName)->clear();
+
+ ScopedRecursiveLockPtr l = getScopedLock();
+ auto& layer=layers.at(layerName);
+ layer.addedCoordVisualizations.clear();
+ layer.addedLineVisualizations.clear();
+ layer.addedBoxVisualizations.clear();
+ layer.addedTextVisualizations.clear();
+ layer.addedSphereVisualizations.clear();
+ layer.mainNode->removeAllChildren();
}
void DebugDrawerComponent::clearDebugLayer(const Ice::Current &)
{
- ARMARX_VERBOSE << "Clearing debug layer...";
-
clearLayer(DEBUG_LAYER_NAME);
}
@@ -469,7 +478,7 @@ SoSeparator* DebugDrawerComponent::getVisualization()
return coinVisu;
}
-std::shared_ptr<DebugDrawerComponent::Layer> DebugDrawerComponent::requestLayer(const std::string& layerName)
+DebugDrawerComponent::Layer& DebugDrawerComponent::requestLayer(const std::string& layerName)
{
ScopedRecursiveLockPtr l = getScopedLock();
if(hasLayer(layerName))
@@ -477,62 +486,14 @@ std::shared_ptr<DebugDrawerComponent::Layer> DebugDrawerComponent::requestLayer(
return layers.at(layerName);
}
ARMARX_VERBOSE << "Created layer "<<layerName;
- layers[layerName]=std::shared_ptr<DebugDrawerComponent::Layer>{new Layer(*this)};
- return layers.at(layerName);
-}
-DebugDrawerComponent::Layer::Layer(DebugDrawerComponent& parentDrawer):
- parent(parentDrawer),
- mainNode{new SoSeparator{}},
- addedCoordVisualizations{},
- addedLineVisualizations{},
- addedBoxVisualizations{},
- addedTextVisualizations{},
- addedSphereVisualizations{},
- visible{true}
-{
+ SoSeparator* mainNode=new SoSeparator{};
mainNode->ref();
- ScopedRecursiveLockPtr l = parent.getScopedLock();
- parent.coinVisu->addChild(mainNode);
-}
-
-DebugDrawerComponent::Layer::Layer(Layer && other):
- parent(other.parent),
- mainNode{other.mainNode},
- addedCoordVisualizations{std::move(other.addedCoordVisualizations)},
- addedLineVisualizations{std::move(other.addedLineVisualizations)},
- addedBoxVisualizations{std::move(other.addedBoxVisualizations)},
- addedTextVisualizations{std::move(other.addedTextVisualizations)},
- addedSphereVisualizations{std::move(other.addedSphereVisualizations)},
- visible{other.visible}
-{
- other.mainNode=nullptr;
-}
-
-DebugDrawerComponent::Layer::~Layer()
-{
- if(mainNode)
- {
- ScopedRecursiveLockPtr l = parent.getScopedLock();
-
- if (parent.coinVisu->findChild(mainNode)>=0)
- {
- parent.coinVisu->removeChild(mainNode);
- }
- clear();
- mainNode->unref();
- }
-}
-
-void DebugDrawerComponent::Layer::clear()
-{
- ScopedRecursiveLockPtr l = parent.getScopedLock();
- addedCoordVisualizations.clear();
- addedLineVisualizations.clear();
- addedBoxVisualizations.clear();
- addedTextVisualizations.clear();
- addedSphereVisualizations.clear();
- mainNode->removeAllChildren();
+ coinVisu->addChild(mainNode);
+ layers[layerName]=Layer();
+ layers.at(layerName).mainNode=mainNode;
+ layers.at(layerName).visible=true;
+ return layers.at(layerName);
}
bool DebugDrawerComponent::hasLayer(const std::string& layerName, const ::Ice::Current&)
@@ -545,9 +506,16 @@ void DebugDrawerComponent::removeLayer(const std::string& layerName, const ::Ice
{
if(!hasLayer(layerName))
{
+ ARMARX_VERBOSE << "Layer " << layerName<<" can't be removed, because it does not exist.";
return;
}
+ ARMARX_VERBOSE << "Removing layer " << layerName;
+
ScopedRecursiveLockPtr l = getScopedLock();
+ auto& layer=layers.at(layerName);
+ coinVisu->removeChild(layer.mainNode);
+ clearLayer(layerName);
+ layer.mainNode->unref();
layers.erase(layerName);
}
@@ -558,19 +526,19 @@ void DebugDrawerComponent::enableLayerVisu(const std::string& layerName, bool vi
return;
}
ScopedRecursiveLockPtr l = getScopedLock();
- auto layer=layers.at(layerName);
- layer->visible=visible;
+ auto& layer=layers.at(layerName);
+ layer.visible=visible;
if (visible)
{
- if (coinVisu->findChild(layer->mainNode)<0)
+ if (coinVisu->findChild(layer.mainNode)<0)
{
- coinVisu->addChild(layer->mainNode);
+ coinVisu->addChild(layer.mainNode);
}
} else
{
- if (coinVisu->findChild(layer->mainNode)>=0)
+ if (coinVisu->findChild(layer.mainNode)>=0)
{
- coinVisu->removeChild(layer->mainNode);
+ coinVisu->removeChild(layer.mainNode);
}
}
}
@@ -597,12 +565,12 @@ StringSequence DebugDrawerComponent::layerNames(const ::Ice::Current&)
ScopedRecursiveLockPtr l = getScopedLock();
for(const auto& layer:layers)
{
- int count = layer.second->addedCoordVisualizations.size()+
- layer.second->addedLineVisualizations.size()+
- layer.second->addedBoxVisualizations.size()+
- layer.second->addedTextVisualizations.size()+
- layer.second->addedSphereVisualizations.size();
- ::armarx::LayerInformation info={layer.first,layer.second->visible,count};
+ int count = layer.second.addedCoordVisualizations.size()+
+ layer.second.addedLineVisualizations.size()+
+ layer.second.addedBoxVisualizations.size()+
+ layer.second.addedTextVisualizations.size()+
+ layer.second.addedSphereVisualizations.size();
+ ::armarx::LayerInformation info={layer.first,layer.second.visible,count};
seq.push_back(info);
}
return seq;
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index 9d76c7139979a3a23eb3e1f59f2d76fade33c01b..cb475529ed0978eb77553866844f17affcfacf10 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -155,21 +155,12 @@ protected:
void removeText(const std::string& layerName, const std::string &name);
void removeSphere(const std::string& layerName, const std::string &name);
+ /**
+ * @brief Contains data for a layer.
+ */
struct Layer
{
- Layer(DebugDrawerComponent& parentDrawer);
- Layer(Layer & other) = delete;
- Layer& operator=(const Layer&) = delete;
- Layer(Layer && other);
- Layer & operator= ( Layer && other)=delete;
- ~Layer();
-
- void clear();
-
- DebugDrawerComponent& parent;
-
SoSeparator* mainNode;
-
std::map<std::string, SoSeparator*> addedCoordVisualizations;
std::map<std::string, SoSeparator*> addedLineVisualizations;
std::map<std::string, SoSeparator*> addedBoxVisualizations;
@@ -184,11 +175,14 @@ protected:
* \param layerName The layer.
* \return The requested layer.
*/
- std::shared_ptr<Layer> requestLayer(const std::string& layerName);
+ Layer &requestLayer(const std::string& layerName);
SoSeparator* coinVisu;
- std::map<const std::string, std::shared_ptr<Layer>> layers;
+ /*!
+ * \brief All existing layers.
+ */
+ std::map<const std::string, Layer> layers;
boost::shared_ptr<boost::recursive_mutex> mutex;
};