Clean up PoolStorage when stopping controller

CMakeLists.txt

@@ -178,7 +178,9 @@ ENDIF(BUILD_PYTHON_INTERFACE)
 
 # Stand alone embedded intepreter with a robot controller.
 add_executable(geometric_simu src/geometric_simu.cpp src/sot_loader.cpp src/sot_loader_basic.cpp)
-pkg_config_use_dependency(geometric_simu roscpp tf)
+pkg_config_use_dependency(geometric_simu tf)
+pkg_config_use_dependency(geometric_simu roscpp)
+pkg_config_use_dependency(geometric_simu dynamic-graph)
 target_link_libraries(geometric_simu  ros_bridge tf ${Boost_LIBRARIES} ${CMAKE_DL_LIBS})
 
 # Sot loader library

src/sot_loader_basic.cpp

@@ -23,6 +23,8 @@
 #include <dynamic_graph_bridge/sot_loader.hh>
 #include "dynamic_graph_bridge/ros_init.hh"
 
+#include <dynamic-graph/pool.h>
+
 // POSIX.1-2001
 #include <dlfcn.h>
 
@@ -211,6 +213,8 @@ void SotLoaderBasic::Initialization()
 
 void SotLoaderBasic::CleanUp()
 {
+  dynamicgraph::PoolStorage::destroy();
+
   /// Uncount the number of access to this library.
   dlclose(sotRobotControllerLibrary_);
 }