diff --git a/maca_common/include/LimitedStack.hpp b/maca_common/include/LimitedStack.hpp
index 46400380493aac26bea01b954bfaeca4220bdc9b..048c4a4cad728ba458b4b114a0dea4891755b9b9 100644
--- a/maca_common/include/LimitedStack.hpp
+++ b/maca_common/include/LimitedStack.hpp
@@ -57,6 +57,9 @@ class LimitedStack
       lastElementIndex = 0;
 
     present[data[lastElementIndex]]--;
+    if (present[data[lastElementIndex]] <= 0)
+      present.erase(present.find(data[lastElementIndex]));
+
     data[lastElementIndex] = elem;
     present[data[lastElementIndex]]++;
   }