cleanup nodes/edges when removing layer

src/dynamic_scene_graph.cpp

@@ -200,6 +200,12 @@ const Layer& DynamicSceneGraph::addLayer(const std::string& name,
 
 void DynamicSceneGraph::removeLayer(LayerId layer, PartitionId partition) {
   if (!partition) {
+    auto it = layers_.find(layer);
+    if (it != layers_.end()) {
+      while (!it->second->nodes_.empty()) {
+        removeNode(it->second->nodes_.begin()->first);
+      }
+    }
     layers_.erase(layer);
   }
 
@@ -208,7 +214,14 @@ void DynamicSceneGraph::removeLayer(LayerId layer, PartitionId partition) {
     return;
   }
 
-  iter->second.erase(partition);
+  auto part_iter = iter->second.find(partition);
+  if (part_iter != iter->second.end()) {
+    while (!part_iter->second->nodes_.empty()) {
+      removeNode(part_iter->second->nodes_.begin()->first);
+    }
+    iter->second.erase(part_iter);
+  }
+
   if (iter->second.empty()) {
     layer_partitions_.erase(iter);
   }