diff --git a/include/iri_base_algorithm/iri_base_algorithm.h b/include/iri_base_algorithm/iri_base_algorithm.h
index cb00d8fccff476a2167b8f24bd181eddef456787..012fb8c6ec562a780a1dd72bb303432504b38078 100644
--- a/include/iri_base_algorithm/iri_base_algorithm.h
+++ b/include/iri_base_algorithm/iri_base_algorithm.h
@@ -320,6 +320,7 @@ IriBaseAlgorithm<Algorithm>::~IriBaseAlgorithm()
   ROS_DEBUG("IriBaseAlgorithm::Destructor");
 
   this->thread_server_->kill_thread(this->main_thread_id_);
+  this->thread_server_->delete_thread(this->main_thread_id_);
 }
 
 template <class Algorithm>