diff --git a/README.md b/README.md
index caaf8d3..b64c557 100644
--- a/README.md
+++ b/README.md
@@ -30,9 +30,12 @@ cd sot_ws/src
 To build this package after installing all the dependencies:
 ```
 cd sot_ws
-colcon build --merge-install --packages-select dynamic_graph_bridge
+colcon build --merge-install --packages-select dynamic_graph_bridge --cmake-args ' -DPYTHON_STANDARD_LAYOUT:BOOL=ON'
 ```
 
+You need to install everything at the same level specifically for the python modules, and in addition it is necessary to install everything with the standard python layout.
+Note that the same option need to be used for all the packages.
+
 ### Test
 
 To test it:
diff --git a/include/dynamic_graph_bridge/ros.hpp b/include/dynamic_graph_bridge/ros.hpp
index bbf1e0e..547761b 100644
--- a/include/dynamic_graph_bridge/ros.hpp
+++ b/include/dynamic_graph_bridge/ros.hpp
@@ -60,13 +60,32 @@ typedef rclcpp::Service<std_srvs::srv::Empty>::SharedPtr EmptyServicePtr;
 typedef std_srvs::srv::Empty EmptyServiceType;
 
 /**
- * @brief rosInit is a global method that uses the structure GlobalRos.
- * Its role is to create the ros::nodeHandle and the ros::spinner once at the
- * first call. It always returns a reference to the node hanlde.
- * @return the reference of GLOBAL_ROS_VAR.node_.
+ * @brief If the node named node_name does not exist create it with a reentrant
+ * group
+ * @param node_name
+ */
+void create_ros_node(std::string& node_name);
+
+/**
+ * @brief Get the node shared pointer providing the node name.
+ * @param node_name
+ * @return A pointer towards the node.
  */
 RosNodePtr get_ros_node(std::string node_name);
 
+/**
+ * @brief Returns a Reentrant Callback group initialized with the ros node
+ * specifed by node_name
+ * @param node_name
+ * @param Returns a shared pointer towards the described callback group of type
+ * reentrant.
+ */
+rclcpp::CallbackGroup::SharedPtr get_callback_group(std::string node_name);
+
+size_t ros_executor_get_nb_threads();
+
+void ros_display_list_of_nodes();
+
 /**
  * @brief Add a ros node to the global executor.
  *
diff --git a/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp b/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp
index 48f6f85..a398549 100644
--- a/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp
+++ b/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp
@@ -85,8 +85,11 @@ class RosPythonInterpreterServer {
   /**
    * @brief start_ros_service advertize the "run_python_command" and
    * "run_python_scripts" ros services
+   * @param qos: Quality of Service
+   * @param callback_group: Callback group
    */
-  void start_ros_service();
+  void start_ros_service(const rclcpp::QoS& qos = rclcpp::ServicesQoS(),
+                         rclcpp::CallbackGroup::SharedPtr group = nullptr);
 
  protected:
   /**
diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
index 3b0ed29..f844ba6 100644
--- a/include/dynamic_graph_bridge/sot_loader.hh
+++ b/include/dynamic_graph_bridge/sot_loader.hh
@@ -18,9 +18,6 @@
 // STL includes
 #include <map>
 
-// Pinocchio includes
-#include <pinocchio/fwd.hpp>
-
 // Boost includes
 #include <boost/foreach.hpp>
 #include <boost/format.hpp>
@@ -83,7 +80,7 @@ class SotLoader : public SotLoaderBasic {
   geometry_msgs::msg::TransformStamped freeFlyerPose_;
 
  public:
-  SotLoader();
+  SotLoader(const std::string &aNodeName = std::string("sot_loader"));
   virtual ~SotLoader();
 
   // \brief Create a thread for ROS and start the control loop.
diff --git a/include/dynamic_graph_bridge/sot_loader_basic.hh b/include/dynamic_graph_bridge/sot_loader_basic.hh
index 35ac763..76187f7 100644
--- a/include/dynamic_graph_bridge/sot_loader_basic.hh
+++ b/include/dynamic_graph_bridge/sot_loader_basic.hh
@@ -18,9 +18,6 @@
 // STL includes
 #include <map>
 
-// Pinocchio includes
-#include <pinocchio/fwd.hpp>
-
 // Boost includes
 #include <boost/foreach.hpp>
 #include <boost/format.hpp>
@@ -41,8 +38,11 @@
 namespace po = boost::program_options;
 namespace dgs = dynamicgraph::sot;
 
-class SotLoaderBasic : public rclcpp::Node {
+class SotLoaderBasic {
  protected:
+  // RosNode
+  dynamic_graph_bridge::RosNodePtr ros_node_;
+
   // Dynamic graph is stopped.
   bool dynamic_graph_stopped_;
 
@@ -84,7 +84,8 @@ class SotLoaderBasic : public rclcpp::Node {
   std::vector<std::string> stateVectorMap_;
 
  public:
-  SotLoaderBasic(const std::string& aNodeName = std::string("SotLoaderBasic"));
+  SotLoaderBasic(
+      const std::string& aNodeName = std::string("sot_loader_basic"));
   virtual ~SotLoaderBasic();
 
   // \brief Read user input to extract the path of the SoT dynamic library.
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c8df4cb..8c6309a 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -106,7 +106,7 @@ if(BUILD_PYTHON_INTERFACE)
 endif(BUILD_PYTHON_INTERFACE)
 
 # Sot loader library
-add_library(sot_loader sot_loader.cpp sot_loader_basic.cpp)
+add_library(sot_loader SHARED sot_loader.cpp sot_loader_basic.cpp)
 target_link_libraries(
   sot_loader Boost::program_options ${catkin_LIBRARIES} ros_bridge
   dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp)
diff --git a/src/ros.cpp b/src/ros.cpp
index c306efc..1283673 100644
--- a/src/ros.cpp
+++ b/src/ros.cpp
@@ -33,7 +33,8 @@ namespace dynamic_graph_bridge {
 /**
  * @brief Shortcut.
  */
-typedef std::map<std::string, RosNodePtr> GlobalListOfRosNodeType;
+typedef std::tuple<RosNodePtr, rclcpp::CallbackGroup::SharedPtr> NodeInfo;
+typedef std::map<std::string, NodeInfo> GlobalListOfRosNodeType;
 
 /**
  * @brief GLOBAL_LIST_OF_ROS_NODE is global variable that acts as a singleton on
@@ -57,7 +58,7 @@ static GlobalListOfRosNodeType GLOBAL_LIST_OF_ROS_NODE;
  */
 class Executor {
  public:
-  Executor() : ros_executor_(rclcpp::ExecutorOptions(), 4) {
+  Executor() : ros_executor_(rclcpp::ExecutorOptions(), 30) {
     is_thread_running_ = false;
     is_spinning_ = false;
     list_node_added_.clear();
@@ -131,6 +132,13 @@ class Executor {
     }
   }
 
+  /**
+   * @brief Return the number of threads
+   */
+  size_t get_number_of_threads() {
+    return ros_executor_.get_number_of_threads();
+  }
+
  private:
   /**
    * @brief Thread callback function
@@ -227,7 +235,7 @@ bool ros_node_exists(std::string node_name) {
       GLOBAL_LIST_OF_ROS_NODE.end()) {
     return false;
   }
-  if (GLOBAL_LIST_OF_ROS_NODE.at(node_name) == nullptr) {
+  if (std::get<0>(GLOBAL_LIST_OF_ROS_NODE.at(node_name)) == nullptr) {
     return false;
   }
   return true;
@@ -241,19 +249,29 @@ ExecutorPtr get_ros_executor() {
   return EXECUTOR;
 }
 
-RosNodePtr get_ros_node(std::string node_name) {
-  ros_init();
+void create_ros_node(std::string& node_name) {
   if (!ros_node_exists(node_name)) {
-    GLOBAL_LIST_OF_ROS_NODE[node_name] = RosNodePtr(nullptr);
-  }
-  if (!GLOBAL_LIST_OF_ROS_NODE[node_name] ||
-      GLOBAL_LIST_OF_ROS_NODE[node_name].get() == nullptr) {
-    /** RosNode instanciation */
-    GLOBAL_LIST_OF_ROS_NODE[node_name] =
+    RosNodePtr a_ros_node_ptr =
         std::make_shared<RosNode>(node_name, "dynamic_graph_bridge");
+    rclcpp::CallbackGroup::SharedPtr a_cb_group =
+        a_ros_node_ptr->create_callback_group(
+            rclcpp::CallbackGroupType::Reentrant);
+    /** RosNode instanciation */
+    GLOBAL_LIST_OF_ROS_NODE[node_name] = NodeInfo(a_ros_node_ptr, a_cb_group);
   }
+}
+RosNodePtr get_ros_node(std::string node_name) {
+  ros_init();
+  create_ros_node(node_name);
   /** Return a reference to the node handle so any function can use it */
-  return GLOBAL_LIST_OF_ROS_NODE[node_name];
+  return std::get<0>(GLOBAL_LIST_OF_ROS_NODE[node_name]);
+}
+
+rclcpp::CallbackGroup::SharedPtr get_callback_group(std::string node_name) {
+  ros_init();
+  create_ros_node(node_name);
+  /** Return a reference to the node handle so any function can use it */
+  return std::get<1>(GLOBAL_LIST_OF_ROS_NODE[node_name]);
 }
 
 void ros_add_node_to_executor(const std::string& node_name) {
@@ -273,6 +291,22 @@ void ros_shutdown() {
   // rclcpp::shutdown();
 }
 
+size_t ros_executor_get_nb_threads() {
+  return get_ros_executor()->get_number_of_threads();
+}
+
+void ros_display_list_of_nodes() {
+  GlobalListOfRosNodeType::iterator ros_node_it =
+      GLOBAL_LIST_OF_ROS_NODE.begin();
+  while (ros_node_it != GLOBAL_LIST_OF_ROS_NODE.end()) {
+    RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+                "ros_display_list_of_nodes: %s/%s",
+                std::get<0>(ros_node_it->second)->get_namespace(),
+                std::get<0>(ros_node_it->second)->get_name());
+    ros_node_it++;
+  }
+}
+
 void ros_clean() {
   ros_stop_spinning();
   GlobalListOfRosNodeType::iterator ros_node_it =
diff --git a/src/ros_python_interpreter_server.cpp b/src/ros_python_interpreter_server.cpp
index 1162626..272e5d4 100644
--- a/src/ros_python_interpreter_server.cpp
+++ b/src/ros_python_interpreter_server.cpp
@@ -25,18 +25,26 @@ RosPythonInterpreterServer::RosPythonInterpreterServer()
 
 RosPythonInterpreterServer::~RosPythonInterpreterServer() {}
 
-void RosPythonInterpreterServer::start_ros_service() {
+void RosPythonInterpreterServer::start_ros_service(
+    const rclcpp::QoS& qos, rclcpp::CallbackGroup::SharedPtr group) {
   run_python_command_callback_t runCommandCb =
       std::bind(&RosPythonInterpreterServer::runCommandCallback, this,
                 std::placeholders::_1, std::placeholders::_2);
   run_python_command_srv_ = ros_node_->create_service<RunPythonCommandSrvType>(
-      "run_python_command", runCommandCb);
+      "/dynamic_graph_bridge/run_python_command", runCommandCb, qos, group);
+  RCLCPP_INFO(
+      rclcpp::get_logger("dynamic_graph_bridge"),
+      "RosPythonInterpreterServer::start_ros_service - run_python_command...");
 
   run_python_file_callback_t runPythonFileCb =
       std::bind(&RosPythonInterpreterServer::runPythonFileCallback, this,
                 std::placeholders::_1, std::placeholders::_2);
   run_python_file_srv_ = ros_node_->create_service<RunPythonFileSrvType>(
-      "run_python_file", runPythonFileCb);
+      "/dynamic_graph_bridge/run_python_file", runPythonFileCb, qos, group);
+
+  RCLCPP_INFO(
+      rclcpp::get_logger("dynamic_graph_bridge"),
+      "RosPythonInterpreterServer::start_ros_service - run_python_file...");
 }
 
 void RosPythonInterpreterServer::runCommandCallback(
@@ -61,7 +69,11 @@ void RosPythonInterpreterServer::runCommandCallback(
 
 void RosPythonInterpreterServer::runPythonFileCallback(
     RunPythonFileRequestPtr req, RunPythonFileResponsePtr res) {
+  RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+              "RosPythonInterpreterServer::runPythonFileCallback- begin");
   run_python_file(req->input, res->result);
+  RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+              "RosPythonInterpreterServer::runPythonFileCallback- end");
 }
 
 void RosPythonInterpreterServer::run_python_command(const std::string& command,
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 6f56066..7bd1178 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -56,8 +56,8 @@ struct DataToLog {
   }
 };
 
-SotLoader::SotLoader()
-    : SotLoaderBasic(),
+SotLoader::SotLoader(const std::string &aNodeName)
+    : SotLoaderBasic(aNodeName),
       sensorsIn_(),
       controlValues_(),
       angleEncoder_(),
@@ -83,10 +83,10 @@ void SotLoader::initializeServices() {
   freeFlyerPose_.header.frame_id = "odom";
   freeFlyerPose_.child_frame_id = "base_link";
 
-  if (not has_parameter("tf_base_link"))
-    declare_parameter("tf_base_link", std::string("base_link"));
+  if (not ros_node_->has_parameter("tf_base_link"))
+    ros_node_->declare_parameter("tf_base_link", std::string("base_link"));
 
-  if (get_parameter("tf_base_link", freeFlyerPose_.child_frame_id)) {
+  if (ros_node_->get_parameter("tf_base_link", freeFlyerPose_.child_frame_id)) {
     RCLCPP_INFO_STREAM(rclcpp::get_logger("dynamic_graph_bridge"),
                        "Publishing " << freeFlyerPose_.child_frame_id << " wrt "
                                      << freeFlyerPose_.header.frame_id);
@@ -98,7 +98,8 @@ void SotLoader::initializeServices() {
   control_.resize(static_cast<std::size_t>(nbOfJoints_));
 
   // Creates a publisher for the free flyer transform.
-  freeFlyerPublisher_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+  freeFlyerPublisher_ =
+      std::make_shared<tf2_ros::TransformBroadcaster>(ros_node_);
 }
 
 void SotLoader::fillSensors(map<string, dgs::SensorValues> &sensorsIn) {
@@ -164,9 +165,6 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
     joint_state_.position[i] = angleEncoder_[i];
   }
 
-  std::cerr << "parallel_joints_to_state_vector_.size(): "
-            << parallel_joints_to_state_vector_.size() << std::endl;
-
   for (unsigned int i = 0; i < parallel_joints_to_state_vector_.size(); i++) {
     joint_state_.position[i + nbOfJoints_std_s] =
         coefficient_parallel_joints_[i] *
@@ -184,7 +182,6 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
     throw anInvalidArgument;
   }
 
-  std::cerr << "Reached poseValue_" << std::endl;
   std::vector<double> poseValue = controlValues["baseff"].getValues();
 
   freeFlyerPose_.transform.translation.x = poseValue[0];
@@ -199,10 +196,10 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
   freeFlyerPose_.header.stamp = joint_state_.header.stamp;
   // Publish
   freeFlyerPublisher_->sendTransform(freeFlyerPose_);
-  std::cerr << "end of readControl" << std::endl;
 }
 
 void SotLoader::setup() {
+  if (sotController_ == NULL) return;
   fillSensors(sensorsIn_);
   sotController_->setupSetSensors(sensorsIn_);
   sotController_->getControl(controlValues_);
@@ -210,6 +207,8 @@ void SotLoader::setup() {
 }
 
 void SotLoader::oneIteration() {
+  if (sotController_ == NULL) return;
+
   fillSensors(sensorsIn_);
   try {
     sotController_->nominalSetSensors(sensorsIn_);
@@ -230,11 +229,13 @@ void SotLoader::workThreadLoader() {
   double periodd;
 
   /// Declare parameters
-  if (not has_parameter("dt")) declare_parameter<double>("dt", 0.01);
-  if (not has_parameter("paused")) declare_parameter<bool>("paused", false);
+  if (not ros_node_->has_parameter("dt"))
+    ros_node_->declare_parameter<double>("dt", 0.01);
+  if (not ros_node_->has_parameter("paused"))
+    ros_node_->declare_parameter<bool>("paused", false);
 
   //
-  get_parameter_or("dt", periodd, 0.001);
+  ros_node_->get_parameter_or("dt", periodd, 0.001);
   rclcpp::Rate rate(1 / periodd);  // 1 kHz
 
   DataToLog dataToLog(5000);
@@ -250,15 +251,15 @@ void SotLoader::workThreadLoader() {
   rclcpp::Time time;
   while (rclcpp::ok() && !isDynamicGraphStopped()) {
     // Check if the user did not paused geometric_simu
-    get_parameter_or("paused", paused, false);
+    ros_node_->get_parameter_or("paused", paused, false);
 
     if (!paused) {
       time = aClock.now();
       oneIteration();
 
       rclcpp::Duration d = aClock.now() - time;
-      dataToLog.record((time - timeOrigin).nanoseconds() * 1.0e9,
-                       d.nanoseconds() * 1.0e9);
+      dataToLog.record((double)((time - timeOrigin).nanoseconds()) * 1.0e9,
+                       (double)(d.nanoseconds()) * 1.0e9);
     }
     rate.sleep();
   }
diff --git a/src/sot_loader_basic.cpp b/src/sot_loader_basic.cpp
index d6023bd..1ac1aea 100644
--- a/src/sot_loader_basic.cpp
+++ b/src/sot_loader_basic.cpp
@@ -8,8 +8,17 @@
 /* -------------------------------------------------------------------------- */
 /* --- INCLUDES ------------------------------------------------------------- */
 /* -------------------------------------------------------------------------- */
-
-#include "dynamic_graph_bridge/sot_loader_basic.hh"
+// This is imposed by pinocchio through the integration of robot-utils.hh
+#ifdef BOOST_MPL_LIMIT_VECTOR_SIZE
+#pragma push_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
+#undef BOOST_MPL_LIMIT_VECTOR_SIZE
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/xml_parser.hpp>
+#pragma pop_macro("BOOST_MPL_LIMIT_VECTOR_SIZE")
+#else
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/xml_parser.hpp>
+#endif
 
 #include <dynamic-graph/pool.h>
 
@@ -19,8 +28,7 @@
 #include <sot/core/robot-utils.hh>
 #include <stdexcept>
 
-#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/parsers/urdf.hpp"
+#include "dynamic_graph_bridge/sot_loader_basic.hh"
 
 // POSIX.1-2001
 #include <dlfcn.h>
@@ -30,10 +38,12 @@ using namespace dynamicgraph::sot;
 namespace po = boost::program_options;
 
 SotLoaderBasic::SotLoaderBasic(const std::string& aNodeName)
-    : rclcpp::Node(aNodeName),
+    : ros_node_(nullptr),
       dynamic_graph_stopped_(true),
-      sotController_(NULL),
+      sotController_(nullptr),
       sotRobotControllerLibrary_(0) {
+  ros_node_ = dynamic_graph_bridge::get_ros_node(aNodeName);
+
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge::"),
               "Beginning of SotLoaderBasic");
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
@@ -51,8 +61,8 @@ int SotLoaderBasic::initPublication() {
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
               "SotLoaderBasic::initPublication - create joint_pub");
 
-  joint_pub_ =
-      create_publisher<sensor_msgs::msg::JointState>("joint_states", 1);
+  joint_pub_ = ros_node_->create_publisher<sensor_msgs::msg::JointState>(
+      "joint_states", 1);
 
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
               "SotLoaderBasic::initPublication - after create joint_pub");
@@ -65,19 +75,19 @@ void SotLoaderBasic::initializeServices() {
       "SotLoaderBasic::initializeServices - Ready to start dynamic graph.");
 
   using namespace std::placeholders;
-  service_start_ = create_service<std_srvs::srv::Empty>(
+  service_start_ = ros_node_->create_service<std_srvs::srv::Empty>(
       "start_dynamic_graph",
       std::bind(&SotLoaderBasic::start_dg, this, std::placeholders::_1,
                 std::placeholders::_2));
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-              "SotLoaderBasic::initializeServices - started dynamic graph.");
+              "SotLoaderBasic::initializeServices - start_dynamic_graph.");
 
-  service_stop_ = create_service<std_srvs::srv::Empty>(
+  service_stop_ = ros_node_->create_service<std_srvs::srv::Empty>(
       "stop_dynamic_graph",
       std::bind(&SotLoaderBasic::stop_dg, this, std::placeholders::_1,
                 std::placeholders::_2));
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-              "SotLoaderBasic::initializeServices - stopped dynamic graph.");
+              "SotLoaderBasic::initializeServices - stop_dynamic_graph.");
 
   parameter_server_read_robot_description();
 }
@@ -92,22 +102,24 @@ int SotLoaderBasic::readSotVectorStateParam() {
 
   // It is necessary to declare parameters first
   // before trying to access them.
-  if (not has_parameter("state_vector_map"))
-    declare_parameter("state_vector_map", std::vector<std::string>{});
-  if (not has_parameter("joint_state_parallel"))
-    declare_parameter("joint_state_parallel", std::vector<std::string>{});
+  if (not ros_node_->has_parameter("state_vector_map"))
+    ros_node_->declare_parameter("state_vector_map",
+                                 std::vector<std::string>{});
+  if (not ros_node_->has_parameter("joint_state_parallel"))
+    ros_node_->declare_parameter("joint_state_parallel",
+                                 std::vector<std::string>{});
 
   // Read the state vector of the robot
   // Defines the order in which the actuators are ordered
   try {
     std::string aParameterName("state_vector_map");
-    if (!get_parameter(aParameterName, stateVectorMap_)) {
+    if (!ros_node_->get_parameter(aParameterName, stateVectorMap_)) {
       logic_error aLogicError(
           "SotLoaderBasic::readSotVectorStateParam : State_vector_map is "
           "empty");
       throw aLogicError;
     }
-    RCLCPP_INFO(get_logger(), "state_vector_map parameter size %ld",
+    RCLCPP_INFO(ros_node_->get_logger(), "state_vector_map parameter size %ld",
                 stateVectorMap_.size());
   } catch (exception& e) {
     std::throw_with_nested(std::logic_error("Unable to call get_parameter"));
@@ -122,7 +134,7 @@ int SotLoaderBasic::readSotVectorStateParam() {
 
   std::string prefix("joint_state_parallel");
   std::map<std::string, rclcpp::Parameter> joint_state_parallel;
-  get_parameters(prefix, joint_state_parallel);
+  ros_node_->get_parameters(prefix, joint_state_parallel);
 
   // Iterates over the map joint_state_parallel
   for (std::map<std::string, rclcpp::Parameter>::iterator it_map_expression =
@@ -205,7 +217,8 @@ void SotLoaderBasic::loadController() {
   sotRobotControllerLibrary_ =
       dlopen(dynamicLibraryName_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
   if (!sotRobotControllerLibrary_) {
-    std::cerr << "Cannot load library: " << dlerror() << '\n';
+    RCLCPP_ERROR(rclcpp::get_logger("dynamic_graph_bridge"),
+                 "Cannot load library: %s", dlerror());
     return;
   }
 
@@ -218,14 +231,16 @@ void SotLoaderBasic::loadController() {
           dlsym(sotRobotControllerLibrary_, "createSotExternalInterface")));
   const char* dlsym_error = dlerror();
   if (dlsym_error) {
-    std::cerr << "Cannot load symbol create: " << dlsym_error << " from "
-              << dynamicLibraryName_ << '\n';
+    RCLCPP_ERROR(rclcpp::get_logger("dynamic_graph_bridge"),
+                 "Cannot load symbol create: %s from %s", dlsym_error,
+                 dynamicLibraryName_.c_str());
     return;
   }
 
   // Create robot-controller
   sotController_ = createSot();
-  cout << "Went out from loadController." << endl;
+  RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+              "Went out from loadController.");
 }
 
 void SotLoaderBasic::CleanUp() {
@@ -272,13 +287,13 @@ void SotLoaderBasic::stop_dg(
 }
 
 bool SotLoaderBasic::parameter_server_read_robot_description() {
-  if (!has_parameter("robot_description")) {
-    declare_parameter("robot_description", std::string(""));
+  if (!ros_node_->has_parameter("robot_description")) {
+    ros_node_->declare_parameter("robot_description", std::string(""));
   }
 
   std::string robot_description;
   std::string parameter_name("robot_description");
-  get_parameter(parameter_name, robot_description);
+  ros_node_->get_parameter(parameter_name, robot_description);
   if (robot_description.empty()) {
     RCLCPP_FATAL(rclcpp::get_logger("dynamic_graph_bridge"),
                  "Parameter robot_description is empty");
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 2a7b4dd..aab0fcf 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -66,6 +66,9 @@ if(BUILD_TESTING)
     test_sot_loader
     PUBLIC include "${GMOCK_INCLUDE_DIRS}"
     PRIVATE ${PROJECT_SOURCE_DIR}/include)
+  target_compile_definitions(
+    test_sot_loader
+    PUBLIC TEST_CONFIG_PATH="${CMAKE_CURRENT_LIST_DIR}/python_scripts/")
   target_link_libraries(test_sot_loader sot_loader "${GMOCK_LIBRARIES}")
 
   add_launch_test(launch/launching_test_sot_loader.py)
@@ -89,6 +92,11 @@ if(BUILD_TESTING)
     # Create the cmake target using ament and gtest.
     ament_add_gtest(test_${test_name} main.cpp test_${test_name}.cpp)
 
+    # add some preprocessor variable
+    target_compile_definitions(
+      test_${test_name}
+      PUBLIC TEST_CONFIG_PATH="${CMAKE_CURRENT_LIST_DIR}/python_scripts/")
+
     # Include dependencies.
     target_include_directories(
       test_${test_name}
@@ -107,10 +115,9 @@ if(BUILD_TESTING)
     ament_target_dependencies(${current_test_name} dynamic_graph_bridge_msgs
                               rclcpp rcl_interfaces std_srvs)
 
-    # add some preprocessor variable
-    target_compile_definitions(
-      test_${test_name}
-      PUBLIC TEST_CONFIG_PATH="${CMAKE_CURRENT_LIST_DIR}/python_scripts/")
+    # Install tests
+    install(TARGETS test_${test_name} DESTINATION lib/${PROJECT_NAME})
+
   endmacro(create_ros_bridge_unittest test_name)
 
   # C++ unit-tests.
diff --git a/tests/impl_test_sot_external_interface.cpp b/tests/impl_test_sot_external_interface.cpp
index e162b67..59fa5fd 100644
--- a/tests/impl_test_sot_external_interface.cpp
+++ b/tests/impl_test_sot_external_interface.cpp
@@ -25,6 +25,7 @@ ImplTestSotExternalInterface::ImplTestSotExternalInterface(
 ImplTestSotExternalInterface::~ImplTestSotExternalInterface() {}
 
 void ImplTestSotExternalInterface::init() {
+  std::cout << "ImplTestSotExternalInterface::init - begin" << std::endl;
   std::vector<double> ctrl_vector;
   ctrl_vector.resize(2);
 
@@ -54,8 +55,25 @@ void ImplTestSotExternalInterface::init() {
   // Set the control time step parameter to 0.001
   double ts = 0.001;
 
+  // Publish parameters related to the control interface
   py_inter_ptr->declare_parameter<double>("/sot_controller/dt", ts);
+  // Create services to interact with the embedded python interpreter.
+
+  rclcpp::CallbackGroup::SharedPtr reentrant_cb_group;
+  reentrant_cb_group = get_callback_group("python_interpreter");
+  py_interpreter_srv_->start_ros_service(rclcpp::ServicesQoS(),
+                                         reentrant_cb_group);
+
+  // Non blocking spinner to deal with ros.
+  // Be careful: here with tests we do not care about real time issue.
+  // In the real robot you need to make sure that the control loop is
+  // not block and that the thread associated with the services is not blocking
+  // the control loop.
+  ros_spin_non_blocking();
+
   device_->timeStep(ts);
+
+  std::cout << "ImplTestSotExternalInterface::init - end" << std::endl;
 }
 
 void ImplTestSotExternalInterface::setupSetSensors(
@@ -65,7 +83,8 @@ void ImplTestSotExternalInterface::setupSetSensors(
 
 void ImplTestSotExternalInterface::nominalSetSensors(
     std::map<std::string, dynamicgraph::sot::SensorValues> &) {
-  std::cout << "ImplTestSotExternalInterface::nominalSetSensors" << std::endl;
+  //  std::cout << "ImplTestSotExternalInterface::nominalSetSensors" <<
+  //  std::endl;
 }
 
 void ImplTestSotExternalInterface::cleanupSetSensors(
diff --git a/tests/impl_test_sot_mock_device-python-module-py.cc b/tests/impl_test_sot_mock_device-python-module-py.cc
index 92b69d2..3969283 100644
--- a/tests/impl_test_sot_mock_device-python-module-py.cc
+++ b/tests/impl_test_sot_mock_device-python-module-py.cc
@@ -13,7 +13,7 @@
 namespace dg = dynamicgraph;
 
 BOOST_PYTHON_MODULE(wrap) {
-  bp::import("dynmic_graph.sot.core.wrap");
+  bp::import("dynamic_graph.sot.core.wrap");
 
   dg::python::exposeEntity<ImplTestSotMockDevice,
                            bp::bases<dg::sot::Device> >();
diff --git a/tests/launch/launching_test_sot_loader.py b/tests/launch/launching_test_sot_loader.py
index 2080ad0..ba3b282 100644
--- a/tests/launch/launching_test_sot_loader.py
+++ b/tests/launch/launching_test_sot_loader.py
@@ -73,7 +73,7 @@ class TestSotLoaderBasic(unittest.TestCase):
 
     def test_termination(self, terminating_process, proc_info):
         """Calls the decorator generate_test_description."""
-        proc_info.assertWaitForShutdown(process=terminating_process, timeout=(10))
+        proc_info.assertWaitForShutdown(process=terminating_process, timeout=(60))
 
 
 @launch_testing.post_shutdown_test()
diff --git a/tests/python_scripts/simple_ros_publish.py b/tests/python_scripts/simple_ros_publish.py
index 3bdc9c8..78729ed 100644
--- a/tests/python_scripts/simple_ros_publish.py
+++ b/tests/python_scripts/simple_ros_publish.py
@@ -4,7 +4,7 @@
 from dynamic_graph.ros.ros_publish import RosPublish
 from dynamic_graph.ros.tests.impl_test_sot_mock_device import ImplTestSotMockDevice
 
-# Create a topic from the SoT to the ROS world
+# # Create a topic from the SoT to the ROS world
 ros_publish = RosPublish("rosPublish")
 
 name = "control"
diff --git a/tests/test_ros_init.cpp b/tests/test_ros_init.cpp
index 1bb8b0c..0366e2c 100644
--- a/tests/test_ros_init.cpp
+++ b/tests/test_ros_init.cpp
@@ -139,7 +139,7 @@ TEST_F(TestRosInit, test_services_shut_down) {
       n1->create_client<std_srvs::srv::Empty>(service_name);
 
   /* Test */
-  ASSERT_FALSE(client->wait_for_service(100ms));
+  ASSERT_FALSE(client->wait_for_service(500ms));
 }
 
 /**
diff --git a/tests/test_ros_interpreter.cpp b/tests/test_ros_interpreter.cpp
index 5ad5523..10091b3 100644
--- a/tests/test_ros_interpreter.cpp
+++ b/tests/test_ros_interpreter.cpp
@@ -43,6 +43,7 @@ class TestRosInterpreter : public ::testing::Test {
     // delete the ros node
     ros_clean();
   }
+
   /**
    * @brief Node name
    */
@@ -271,7 +272,6 @@ TEST_F(TestRosInterpreter, test_call_run_script_standarderror) {
 
 TEST_F(TestRosInterpreter, test_call_run_script_ros_publish) {
   /* Setup. */
-
   // Create the ros python interpreter.
   RosPythonInterpreterServer rpi;
   rpi.start_ros_service();
@@ -284,6 +284,6 @@ TEST_F(TestRosInterpreter, test_call_run_script_ros_publish) {
   std::string standard_error = "";
   start_run_python_script_ros_service(file_name, result);
 
-  /* Tests. */
+  /* Tests the result. */
   ASSERT_EQ(result, "");
 }
diff --git a/tests/test_sot_loader.cpp b/tests/test_sot_loader.cpp
index 59ca291..f3c95f9 100644
--- a/tests/test_sot_loader.cpp
+++ b/tests/test_sot_loader.cpp
@@ -1,29 +1,142 @@
-
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Wdeprecated-copy"
 #include <gmock/gmock.h>
+#pragma clang diagnostic pop
+
+#include <chrono>
+
+using namespace std::chrono_literals;
+
+#include <rclcpp/rclcpp.hpp>
 
 #include "dynamic_graph_bridge/ros.hpp"
+#include "dynamic_graph_bridge/ros_python_interpreter_server.hpp"
 #include "dynamic_graph_bridge/sot_loader.hh"
+#include "dynamic_graph_bridge_msgs/msg/vector.hpp"
+
+namespace test_sot_loader {
+int l_argc;
+char** l_argv;
+}  // namespace test_sot_loader
+
+namespace dg = dynamicgraph;
 
 class MockSotLoaderTest : public ::testing::Test {
  public:
   class MockSotLoader : public SotLoader {
    public:
-    ~MockSotLoader() {}
+    rclcpp::Subscription<dynamic_graph_bridge_msgs::msg::Vector>::SharedPtr
+        subscription_;
+    bool service_done_;
+    std::string node_name_;
+
+    rclcpp::CallbackGroup::SharedPtr reentrant_cb_group_;
+
+    MockSotLoader() : node_name_("unittest_sot_loader") {
+      reentrant_cb_group_ =
+          dynamic_graph_bridge::get_ros_node(node_name_)
+              ->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+    }
+
+    ~MockSotLoader() { service_done_ = false; }
+
+    void topic_callback(
+        const dynamic_graph_bridge_msgs::msg::Vector::SharedPtr msg) const {
+      bool ok = true;
+      for (std::vector<double>::size_type idx = 0; idx < msg->data.size();
+           idx++) {
+        if (msg->data[idx] != 0.0) ok = false;
+      }
+      ASSERT_TRUE(ok);
+    }
+    // This method is to listen to publication from the control signal (dg
+    // world) to the control_ros topic (ros world).
+    void subscribe_to_a_topic() {
+      rclcpp::SubscriptionOptions subscription_options;
+      subscription_options.callback_group = reentrant_cb_group_;
+      subscription_ =
+          dynamic_graph_bridge::get_ros_node(node_name_)
+              ->create_subscription<dynamic_graph_bridge_msgs::msg::Vector>(
+                  "control_ros", 1,
+                  std::bind(&MockSotLoader::topic_callback, this,
+                            std::placeholders::_1),
+                  subscription_options);
+    }
+
+    void start_run_python_script_ros_service(const std::string& file_name) {
+      // Service name.
+      std::string service_name = "/dynamic_graph_bridge/run_python_file";
+      // Create a client from a temporary node.
+      rclcpp::CallbackGroup::SharedPtr local_cb_group =
+          dynamic_graph_bridge::get_callback_group("python_interpreter");
+      auto client =
+          dynamic_graph_bridge::get_ros_node("python_interpreter")
+              ->create_client<dynamic_graph_bridge_msgs::srv::RunPythonFile>(
+                  service_name, rclcpp::ServicesQoS(), local_cb_group);
+      ASSERT_TRUE(client->wait_for_service(1s));
+
+      // Fill the command message.
+      dynamic_graph_bridge_msgs::srv::RunPythonFile::Request::SharedPtr
+          request = std::make_shared<
+              dynamic_graph_bridge_msgs::srv::RunPythonFile::Request>();
+      request->input = file_name;
+      // Call the service.
+      auto response = client->async_send_request(request);
+      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+                  "Send request to service %s - Waiting", service_name.c_str());
+      response.wait();
+      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+                  "start_run_python_script_ros_service - Result: %s",
+                  response.get()->result.c_str());
+    }
+
+    void display_services(std::map<std::string, std::vector<std::string>>&
+                              list_service_and_types) {
+      for (std::map<std::string, std::vector<std::string>>::const_iterator
+               const_it = list_service_and_types.begin();
+           const_it != list_service_and_types.end(); ++const_it) {
+        std::cerr << const_it->first << "\t\t\t";
+        for (unsigned i = 0; i < const_it->second.size(); ++i) {
+          std::cerr << std::right;
+          std::cerr << const_it->second[i] << std::endl;
+        }
+      }
+    }
+
+    void local_ros_spin() {
+      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"), "local_ros_spin");
+      dynamic_graph_bridge::ros_spin();
+    }
 
     void generateEvents() {
       usleep(20000);
-      std::cerr << "Start Dynamic Graph " << std::endl;
+      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+                  "Start Dynamic Graph ");
       dynamic_graph_stopped_ = false;
 
-      usleep(20000);
-      std::cerr << "Stop Dynamic Graph " << std::endl;
+      std::string file_name =
+          TEST_CONFIG_PATH + std::string("simple_ros_publish.py");
+      std::string result = "";
+      std::string standard_output = "";
+      std::string standard_error = "";
+      start_run_python_script_ros_service(file_name);
+
+      usleep(100000);
+      std::map<std::string, std::vector<std::string>> list_service_and_types;
+      dynamic_graph_bridge::RosNodePtr ros_node =
+          dynamic_graph_bridge::get_ros_node(node_name_);
+      bool simple_service_exists = false;
+
+      usleep(30720000);  // Wait 30s
+      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+                  "Stop Dynamic Graph ");
       dynamic_graph_stopped_ = true;
     }
 
     void testLoadController() {
       // Set input  call
       int argc = 2;
-      char *argv[2];
+      char* argv[2];
       char argv1[30] = "mocktest";
       argv[0] = argv1;
       char argv2[60] = "--input-file=libimpl_test_library.so";
@@ -33,6 +146,7 @@ class MockSotLoaderTest : public ::testing::Test {
       std::string finalname("libimpl_test_library.so");
       EXPECT_TRUE(finalname == dynamicLibraryName_);
 
+      readSotVectorStateParam();
       // Performs initialization of libimpl_test_library.so
       loadController();
       EXPECT_TRUE(sotRobotControllerLibrary_ != 0);
@@ -50,27 +164,39 @@ class MockSotLoaderTest : public ::testing::Test {
 
       // Start the control loop thread.
       startControlLoop();
-
       // Start the thread generating events.
       std::thread local_events(&MockSotLoader::generateEvents, this);
 
+      // Start the thread for ros events.
+      std::thread local_ros_spin_thread(&MockSotLoader::local_ros_spin, this);
+
       // Wait for each threads.
-      SotLoader::lthread_join();  // Wait 100 ms
+      SotLoader::lthread_join();
       local_events.join();
     }
   };
 
  public:
-  MockSotLoader *mockSotLoader_ptr_;
+  MockSotLoader* mockSotLoader_ptr_;
+
+  // For the set of tests coded in this file.
+  static void SetUpTestCase() {
+    rclcpp::init(test_sot_loader::l_argc, test_sot_loader::l_argv);
+  }
+
+  // For each test specified in this file
+  static void TearDownTestCase() { rclcpp::shutdown(); }
 
   void SetUp() {
     mockSotLoader_ptr_ = new MockSotLoader();
     mockSotLoader_ptr_->initialize();
+    dynamic_graph_bridge::ros_spin_non_blocking();
   }
 
   void TearDown() {
     delete mockSotLoader_ptr_;
     mockSotLoader_ptr_ = nullptr;
+    dynamic_graph_bridge::ros_clean();
   }
 };
 
@@ -78,12 +204,13 @@ TEST_F(MockSotLoaderTest, TestLoadController) {
   mockSotLoader_ptr_->testLoadController();
 }
 
-int main(int argc, char **argv) {
+int main(int argc, char** argv) {
   ::testing::InitGoogleTest(&argc, argv);
-  rclcpp::init(argc, argv);
+
+  test_sot_loader::l_argc = argc;
+  test_sot_loader::l_argv = argv;
 
   int r = RUN_ALL_TESTS();
 
-  rclcpp::shutdown();
   return r;
 }