diff --git a/mbf_costmap_nav/src/mbf_costmap_nav/costmap_wrapper.cpp b/mbf_costmap_nav/src/mbf_costmap_nav/costmap_wrapper.cpp
index 05899bab..50d89bbb 100644
--- a/mbf_costmap_nav/src/mbf_costmap_nav/costmap_wrapper.cpp
+++ b/mbf_costmap_nav/src/mbf_costmap_nav/costmap_wrapper.cpp
@@ -64,6 +64,7 @@ CostmapWrapper::CostmapWrapper(const std::string &name, const TFPtr &tf_listener
 
 CostmapWrapper::~CostmapWrapper()
 {
+  shutdown_costmap_timer_.stop();
 }
 
 
diff --git a/mbf_costmap_nav/src/move_base_server_node.cpp b/mbf_costmap_nav/src/move_base_server_node.cpp
index edec7d9d..668b5b4a 100644
--- a/mbf_costmap_nav/src/move_base_server_node.cpp
+++ b/mbf_costmap_nav/src/move_base_server_node.cpp
@@ -75,5 +75,9 @@ int main(int argc, char **argv)
 #endif
   costmap_nav_srv_ptr = boost::make_shared<mbf_costmap_nav::CostmapNavigationServer>(tf_listener_ptr);
   ros::spin();
+
+  // explicitly call destructor here, otherwise costmap_nav_srv_ptr will be
+  // destructed after tearing down internally allocated static variables
+  costmap_nav_srv_ptr.reset();
   return EXIT_SUCCESS;
 }