DebugDrawer: Adapted sphere drawing to new DebugDrawer

source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp

@@ -211,7 +211,7 @@ void DebugDrawerComponent::drawText(const std::string &layerName, const std::str
     layer->mainNode->addChild(sep);
 }
 
-void DebugDrawerComponent::drawSphere(const std::string &name, const Eigen::Vector3f &position, const VisualizationFactory::Color &color, float radius)
+void DebugDrawerComponent::drawSphere(const std::string& layerName, const std::string &name, const Eigen::Vector3f &position, const VisualizationFactory::Color &color, float radius)
 {
     ScopedRecursiveLockPtr l = getScopedLock();
     ARMARX_DEBUG << "drawSphere";
@@ -235,8 +235,8 @@ void DebugDrawerComponent::drawSphere(const std::string &name, const Eigen::Vect
     sphere->radius = radius;
     sep->addChild(sphere);
 
-    addedSphereVisualizations[name] = sep;
-    debugVisu->addChild(sep);
+    layer->addedSphereVisualizations[name] = sep;
+    layer->mainNode->addChild(sep);
 }
 
 
@@ -291,16 +291,21 @@ void DebugDrawerComponent::removeText(const std::string &layerName, const std::s
     layer->addedTextVisualizations.erase(name);
 }
 
-void DebugDrawerComponent::removeSphere(const std::string &name)
+void DebugDrawerComponent::removeSphere(const std::string &layerName, const std::string &name)
 {
+    if(!hasLayer(layerName))
+    {
+        return;
+    }
     ScopedRecursiveLockPtr l = getScopedLock();
-    if(addedTextVisualizations.find(name) == addedTextVisualizations.end())
+    auto layer=layers.at(layerName);
+    if(layer->addedSphereVisualizations.find(name) == layer->addedSphereVisualizations.end())
     {
         return;
     }
 
-    debugVisu->removeChild(addedSphereVisualizations[name]);
-    addedSphereVisualizations.erase(name);
+    layer->mainNode->removeChild(layer->addedSphereVisualizations[name]);
+    layer->addedSphereVisualizations.erase(name);
 }
 
 
@@ -401,7 +406,7 @@ void DebugDrawerComponent::setSphereVisu(const std::string &layerName, const std
 {
     ARMARX_DEBUG << VAROUT(layerName) << VAROUT(sphereName);
 
-    drawSphere(sphereName, Vector3Ptr::dynamicCast(globalPosition)->toEigen(), VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a), radius);
+    drawSphere(layerName, sphereName, Vector3Ptr::dynamicCast(globalPosition)->toEigen(), VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a), radius);
 }
 
 void DebugDrawerComponent::setSphereDebugLayerVisu(const std::string &sphereName, const Vector3BasePtr &globalPosition, const DrawColor &color, float radius, const Ice::Current &)
@@ -411,7 +416,7 @@ void DebugDrawerComponent::setSphereDebugLayerVisu(const std::string &sphereName
 
 void DebugDrawerComponent::removeSphereVisu(const std::string &layerName, const std::string &sphereName, const Ice::Current &)
 {
-    removeSphere(sphereName);
+    removeSphere(layerName, sphereName);
 }
 
 void DebugDrawerComponent::removeSphereDebugLayerVisu(const std::string &sphereName, const Ice::Current &)

source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h

@@ -156,6 +156,7 @@ protected:
     void removeLine(const std::string& layerName, const std::string &name);
     void removeBox(const std::string& layerName, const std::string &name);
     void removeText(const std::string& layerName, const std::string &name);
+    void removeSphere(const std::string& layerName, const std::string &name);
 
     struct Layer
     {