diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3909a2050a3831e4e178fbedae4f4996b0cdb662..22592403c73338f3038f2c1d31c2c74f32657a81 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -24,8 +24,6 @@ set(CATKIN_ENABLE_TESTING OFF)
 # JRL-cmakemodule setup
 include(cmake/base.cmake)
 include(cmake/boost.cmake)
-include(cmake/python.cmake)
-include(cmake/ros.cmake)
 
 # Project definition
 compute_project_args(PROJECT_ARGS LANGUAGES CXX)
@@ -44,13 +42,13 @@ find_package(std_srvs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(urdfdom REQUIRED)
 
 add_project_dependency(Boost REQUIRED COMPONENTS program_options)
 add_project_dependency(dynamic_graph_bridge_msgs 0.3.0 REQUIRED)
 add_project_dependency(sot-core REQUIRED)
 
 if(BUILD_PYTHON_INTERFACE)
-  findpython()
   search_for_boost_python()
   string(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
   add_project_dependency(dynamic-graph-python 4.0.0 REQUIRED)
diff --git a/include/dynamic_graph_bridge/ros.hpp b/include/dynamic_graph_bridge/ros.hpp
index 57797d22fe6eff9cad0688a307335d00329b6b37..34ff7ebdef8fd90191bd50cbe95ba16316cc0fcc 100644
--- a/include/dynamic_graph_bridge/ros.hpp
+++ b/include/dynamic_graph_bridge/ros.hpp
@@ -9,6 +9,8 @@
 
 #include <memory>
 
+#include <boost/chrono.hpp>
+
 // ROS includes
 #include "dynamic_graph_bridge_msgs/srv/run_python_command.hpp"
 #include "dynamic_graph_bridge_msgs/srv/run_python_file.hpp"
diff --git a/include/dynamic_graph_bridge/sot_loader_basic.hh b/include/dynamic_graph_bridge/sot_loader_basic.hh
index 9f1fd2db14b96c29ff26d87deddec29974e0c0ec..8390448e5fa9cfb0c7b5cce06014bb54d0d0abd8 100644
--- a/include/dynamic_graph_bridge/sot_loader_basic.hh
+++ b/include/dynamic_graph_bridge/sot_loader_basic.hh
@@ -41,7 +41,7 @@
 namespace po = boost::program_options;
 namespace dgs = dynamicgraph::sot;
 
-class SotLoaderBasic {
+class SotLoaderBasic : public rclcpp::Node {
  protected:
   // Dynamic graph is stopped.
   bool dynamic_graph_stopped_;
@@ -73,9 +73,6 @@ class SotLoaderBasic {
   // Joint state publication.
   rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_pub_;
 
-  // Node reference
-  rclcpp::Node::SharedPtr nh_;
-
   // Joint state to be published.
   sensor_msgs::msg::JointState joint_state_;
 
@@ -87,7 +84,7 @@ class SotLoaderBasic {
   std::vector<std::string> stateVectorMap_;
 
  public:
-  SotLoaderBasic();
+  SotLoaderBasic(const std::string & aNodeName=std::string("SotLoaderBasic"));
   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 41a1402c498fa7588326f4ca1b9a01acfeba3d26..96e72da7873024275ae38801a367637e356d6154 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -38,7 +38,9 @@ ament_target_dependencies(
   geometry_msgs
   sensor_msgs
   tf2_ros
-  rcutils)
+  rcutils
+  urdfdom)
+
 if(SUFFIX_SO_VERSION)
   set_target_properties(ros_bridge PROPERTIES SOVERSION ${PROJECT_VERSION})
 endif(SUFFIX_SO_VERSION)
@@ -62,9 +64,12 @@ foreach(plugin ${plugins})
                                                             ${PROJECT_VERSION})
   endif(SUFFIX_SO_VERSION)
   target_link_libraries(
-    ${plugin_library_name} ${${plugin_library_name}_deps} ${catkin_LIBRARIES}
-    ros_bridge
-    dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp
+     ${plugin_library_name} ${${plugin_library_name}_deps} 
+     ros_bridge
+     dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp
+     rclcpp::rclcpp
+     rcl_interfaces::rcl_interfaces__rosidl_typesupport_cpp
+     std_srvs::std_srvs__rosidl_typesupport_cpp
   )
   target_include_directories(
     ${plugin_library_name}
diff --git a/src/dg_ros_mapping.cpp b/src/dg_ros_mapping.cpp
index 5316817f5fb63884a8b6ac10c10ad951a0037853..aae712369f4360eaf30a7d29a10375cc91194469 100644
--- a/src/dg_ros_mapping.cpp
+++ b/src/dg_ros_mapping.cpp
@@ -66,22 +66,25 @@ DG_TO_ROS_IMPL(DgRosMappingMatrix) {
   // For simplicity and easiness of maintenance we implement a slower version
   // of the copy here.
   ros_dst.width = static_cast<unsigned int>(dg_src.rows());
-  ros_dst.data.resize(dg_src.size());
-  for (int row = 0; row < dg_src.rows(); ++row) {
+  ros_dst.data.resize(static_cast<std::size_t>(dg_src.size()));
+  for (int row = 0 ; row < dg_src.rows(); ++row) {
     for (int col = 0; col < dg_src.cols(); ++col) {
-      ros_dst.data[row * ros_dst.width + col] = dg_src(row, col);
+      ros_dst.data[static_cast<unsigned int>(row) * ros_dst.width +
+                   static_cast<unsigned int>(col)] =
+          dg_src(row, col);
     }
   }
 }
 ROS_TO_DG_IMPL(DgRosMappingMatrix) {
   // For simplicity and easiness of maintenance we implement a slower version
   // of the copy here.
-  int rows = ros_src.width;
+  int rows = static_cast<int>(ros_src.width);
   int cols = static_cast<int>(ros_src.data.size()) / static_cast<int>(rows);
   dg_dst.resize(rows, cols);
   for (int row = 0; row < dg_dst.rows(); ++row) {
     for (int col = 0; col < dg_dst.cols(); ++col) {
-      dg_dst(row, col) = ros_src.data[row * rows + col];
+      dg_dst(row, col) =
+          ros_src.data[static_cast<std::vector<double>::size_type>(row * rows + col)];
     }
   }
 }
diff --git a/src/dg_ros_mapping.hpp b/src/dg_ros_mapping.hpp
index c1715f9a185810909f1a7cf9fd092b7ac965eab3..b9dd8db479402f72a55f5f3d99ef909969127c07 100644
--- a/src/dg_ros_mapping.hpp
+++ b/src/dg_ros_mapping.hpp
@@ -8,11 +8,15 @@
  */
 #pragma once
 
+#include "fwd.hpp"
+
 // Standard includes
 #include <sstream>
 #include <utility>
 #include <vector>
 
+#include <boost/chrono.hpp>
+
 // Dynamic Graph types.
 #include <dynamic-graph/linear-algebra.h>
 #include <dynamic-graph/signal-ptr.h>
@@ -107,7 +111,7 @@ class DgRosMapping {
   /** @brief Output signal type. */
   typedef dg::SignalTimeDependent<dg_t, dg::sigtime_t> signal_out_t;
   /** @brief Output signal type. */
-  typedef dg::SignalTimeDependent<timestamp_t, dg::sigtime_t>
+  typedef dg::SignalTimeDependent<dynamic_graph_bridge::timestamp_t, dg::sigtime_t>
       signal_timestamp_out_t;
   /** @brief Input signal type. */
   typedef dg::SignalPtr<dg_t, dg::sigtime_t> signal_in_t;
@@ -126,7 +130,7 @@ class DgRosMapping {
    */
   template <typename SignalTypePtr>
   static void set_default(SignalTypePtr s) {
-    s->setConstant(DgRosMapping<RosType, DgType>::default_value);
+    s->setConstant(default_value);
   }
 
   /**
@@ -135,17 +139,17 @@ class DgRosMapping {
    * @param s
    */
   static void set_default(dg_t& d) {
-    d = DgRosMapping<RosType, DgType>::default_value;
+    d = default_value;
   }
 
   /**
-   * @brief Convert ROS time to std::chrono.
+   * @brief Convert ROS time to boost::chrono.
    *
    * @param ros_time
    * @return timestamp_t
    */
   static timestamp_t from_ros_time(rclcpp::Time ros_time) {
-    return epoch_time() + std::chrono::nanoseconds(ros_time.nanoseconds());
+    return epoch_time() + boost::chrono::nanoseconds(ros_time.nanoseconds());
   }
 
   /**
@@ -154,7 +158,7 @@ class DgRosMapping {
    * @return timestamp_t
    */
   static timestamp_t epoch_time() {
-    return std::chrono::time_point<std::chrono::high_resolution_clock>{};
+    return boost::chrono::time_point<boost::chrono::high_resolution_clock>{};
   }
 
   /**
diff --git a/src/fwd.hpp b/src/fwd.hpp
index cf862b155f9e8797310f6201f077ce10cb14b4b4..bb7d3ab5009746a66c021756e333e557a99b1645 100644
--- a/src/fwd.hpp
+++ b/src/fwd.hpp
@@ -12,14 +12,16 @@
 
 #include <chrono>
 #include <ostream>
+#include <iostream>
+#include <boost/chrono.hpp>
 
 namespace dynamic_graph_bridge {
 /** @brief Time stamp type. */
-typedef std::chrono::time_point<std::chrono::high_resolution_clock> timestamp_t;
+typedef boost::chrono::time_point<boost::chrono::high_resolution_clock> timestamp_t;
 
 }  // namespace dynamic_graph_bridge
 
-namespace dynamicgraph {
+//namespace dynamicgraph {
 /**
  * @brief out stream the time stamp data.
  *
@@ -30,13 +32,16 @@ namespace dynamicgraph {
  * For clang this function needs to be forward declared before the template
  * using it. This is more in accordance to the standard.
  */
-inline std::ostream &operator<<(
-    std::ostream &os, const dynamic_graph_bridge::timestamp_t &time_stamp) {
-  std::chrono::time_point<std::chrono::high_resolution_clock,
-                          std::chrono::milliseconds>
+std::ostream &operator<<(
+    std::ostream &os,
+    const dynamic_graph_bridge::timestamp_t &time_stamp) {
+  boost::chrono::time_point<boost::chrono::high_resolution_clock,
+                          boost::chrono::milliseconds>
       time_stamp_nanosec =
-          std::chrono::time_point_cast<std::chrono::milliseconds>(time_stamp);
+      boost::chrono::time_point_cast<boost::chrono::milliseconds>(time_stamp);
   os << time_stamp_nanosec.time_since_epoch().count();
   return os;
 }
-}  // namespace dynamicgraph
+
+
+//}  // namespace dynamicgraph
diff --git a/src/ros_parameter.cpp b/src/ros_parameter.cpp
index 3cf1e7c2a0eab30ffdf053c03061ff023f019556..79042892c0974bb8b7669bd6cda5d790b8dafd84 100644
--- a/src/ros_parameter.cpp
+++ b/src/ros_parameter.cpp
@@ -38,7 +38,7 @@ bool parameter_server_read_robot_description(rclcpp::Node::SharedPtr nh) {
     // Then set the robot model.
     aRobotUtil->set_parameter(parameter_name, robot_description);
     RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-                "Set parameter_name : %s.", parameter_name.c_str());
+                "parameter_server_read_robot_description : Set parameter_name : %s.", parameter_name.c_str());
     // Everything went fine.
     return true;
   }
diff --git a/src/ros_python_interpreter_client.cpp b/src/ros_python_interpreter_client.cpp
index 9150ff04d8220a9803e8f32db91b5a7ccea4b60a..1a9c05296693ed771c8c944f4a0604ae536a1207 100644
--- a/src/ros_python_interpreter_client.cpp
+++ b/src/ros_python_interpreter_client.cpp
@@ -71,7 +71,7 @@ std::string RosPythonInterpreterClient::run_python_command(
       return_string += response.get()->result;
     }
   } catch (const std::exception& ex) {
-    RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"), ex.what());
+    RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"), "%s", ex.what());
     connect_to_rosservice_run_python_command(timeout_connection_s_);
   } catch (...) {
     RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
diff --git a/src/ros_python_interpreter_server.cpp b/src/ros_python_interpreter_server.cpp
index 22c2d2361911d04c75bee72f06a7671d496cd882..116262661148958c90c09611809ce378bb785b1d 100644
--- a/src/ros_python_interpreter_server.cpp
+++ b/src/ros_python_interpreter_server.cpp
@@ -14,7 +14,6 @@
 #include <memory>
 
 namespace dynamic_graph_bridge {
-static const int queueSize = 5;
 
 RosPythonInterpreterServer::RosPythonInterpreterServer()
     : interpreter_(),
diff --git a/src/ros_subscribe.cpp b/src/ros_subscribe.cpp
index 093fb3e612308ce22b1cca571d5b3fce22fba8ae..ea750604689f6370db2abc7b200165faf7a97b31 100644
--- a/src/ros_subscribe.cpp
+++ b/src/ros_subscribe.cpp
@@ -6,12 +6,12 @@
  * Gesellschaft.
  * @date 2019-05-22
  */
+#include "fwd.hpp"
 
 #include "ros_subscribe.hpp"
 
 #include <dynamic-graph/factory.h>
 
-#include "fwd.hpp"
 
 namespace dynamic_graph_bridge {
 /*
diff --git a/src/ros_subscribe.hpp b/src/ros_subscribe.hpp
index 7be1d0e045c28526833a6d32e19622afb7e7623d..4285d2b1048f165cb416d84e4b4e54ca216fdf6e 100644
--- a/src/ros_subscribe.hpp
+++ b/src/ros_subscribe.hpp
@@ -127,6 +127,7 @@ class RosSubscribe : public dynamicgraph::Entity {
   std::map<std::string, BindedSignal> binded_signals_;
 };
 
+
 namespace command {
 namespace ros_subscribe {
 using ::dynamicgraph::command::Command;
diff --git a/src/ros_subscribe.hxx b/src/ros_subscribe.hxx
index bd673bce41523e022240bee042e0c06c95b03c7c..407a4a02179c168c79fbfe58886b92fa9f1b95fd 100644
--- a/src/ros_subscribe.hxx
+++ b/src/ros_subscribe.hxx
@@ -74,6 +74,7 @@ void RosSubscribe::add(const std::string& signal_name,
 
   // Initialize the time stamp signal if needed.
   std::shared_ptr<signal_timestamp_out_t> signal_timestamp_ptr = nullptr;
+  /*
   if (message_filters::message_traits::HasHeader<ros_t>()) {
     std::string full_time_stamp_signal_name =
         this->getClassName() + "(" + this->getName() + ")::" + signal_name +
@@ -86,6 +87,7 @@ void RosSubscribe::add(const std::string& signal_name,
         dynamicgraph::TimeDependency<dg::sigtime_t>::ALWAYS_READY);
     this->signalRegistration(*signal_timestamp_ptr);
   }
+  */
   std::get<1>(binded_signal) = signal_timestamp_ptr;
 
   // Initialize the subscriber.
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 923a2a6e31d8f329a791c2d616afbe8527cdb150..bb7a5390a4cf0320bb406e6c28527f1d85216448 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -83,16 +83,10 @@ void SotLoader::initializeServices() {
   freeFlyerPose_.header.frame_id = "odom";
   freeFlyerPose_.child_frame_id = "base_link";
 
-  if (nh_ == 0) {
-    logic_error aLogicError(
-        "SotLoaderBasic::initializeFromRosContext aRosCtxt is empty !");
-    throw aLogicError;
-  }
-
-  if (not nh_->has_parameter("tf_base_link"))
-    nh_->declare_parameter("tf_base_link", std::string("base_link"));
+  if (not has_parameter("tf_base_link"))
+    declare_parameter("tf_base_link", std::string("base_link"));
 
-  if (nh_->get_parameter("tf_base_link", freeFlyerPose_.child_frame_id)) {
+  if (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);
@@ -104,7 +98,7 @@ 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>(nh_);
+  freeFlyerPublisher_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
 }
 
 void SotLoader::fillSensors(map<string, dgs::SensorValues> &sensorsIn) {
@@ -142,10 +136,12 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
   }
   sotDEBUG(30) << "End ControlValues" << std::endl;
 #endif
+  std::size_t nbOfJoints_std_s = static_cast<std::size_t>(nbOfJoints_);
+  
   // Check if the size if coherent with the robot description.
   if (control_.size() != (unsigned int)nbOfJoints_) {
-    nbOfJoints_ = control_.size();
-    angleEncoder_.resize(nbOfJoints_);
+    nbOfJoints_ = static_cast<int>(control_.size());
+    angleEncoder_.resize(nbOfJoints_std_s);
   }
 
   // Publish the data.
@@ -154,16 +150,17 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
   joint_state_.header.stamp = rclcpp::Clock().now();
 
   if (joint_state_.position.size() !=
-      nbOfJoints_ + parallel_joints_to_state_vector_.size()) {
-    joint_state_.position.resize(nbOfJoints_ +
+      static_cast<std::size_t>(nbOfJoints_) +
+      parallel_joints_to_state_vector_.size()) {
+    joint_state_.position.resize(nbOfJoints_std_s +
                                  parallel_joints_to_state_vector_.size());
-    joint_state_.velocity.resize(nbOfJoints_ +
+    joint_state_.velocity.resize(nbOfJoints_std_s +
                                  parallel_joints_to_state_vector_.size());
-    joint_state_.effort.resize(nbOfJoints_ +
+    joint_state_.effort.resize(nbOfJoints_std_s +
                                parallel_joints_to_state_vector_.size());
   }
 
-  for (int i = 0; i < nbOfJoints_; i++) {
+  for (std::size_t i = 0; i < nbOfJoints_std_s; i++) {
     joint_state_.position[i] = angleEncoder_[i];
   }
 
@@ -171,9 +168,9 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
             << 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_] =
+    joint_state_.position[i + nbOfJoints_std_s] =
         coefficient_parallel_joints_[i] *
-        angleEncoder_[parallel_joints_to_state_vector_[i]];
+        angleEncoder_[static_cast<std::size_t>(parallel_joints_to_state_vector_[i])];
   }
 
   joint_pub_->publish(joint_state_);
@@ -232,12 +229,12 @@ void SotLoader::workThreadLoader() {
   double periodd;
 
   /// Declare parameters
-  if (not nh_->has_parameter("dt")) nh_->declare_parameter<double>("dt", 0.01);
-  if (not nh_->has_parameter("paused"))
-    nh_->declare_parameter<bool>("paused", false);
+  if (not has_parameter("dt")) declare_parameter<double>("dt", 0.01);
+  if (not has_parameter("paused"))
+    declare_parameter<bool>("paused", false);
 
   //
-  nh_->get_parameter_or("dt", periodd, 0.001);
+  get_parameter_or("dt", periodd, 0.001);
   rclcpp::Rate rate(1 / periodd);  // 1 kHz
 
   DataToLog dataToLog(5000);
@@ -253,7 +250,7 @@ void SotLoader::workThreadLoader() {
   rclcpp::Time time;
   while (rclcpp::ok() && !isDynamicGraphStopped()) {
     // Check if the user did not paused geometric_simu
-    nh_->get_parameter_or("paused", paused, false);
+    get_parameter_or("paused", paused, false);
 
     if (!paused) {
       time = aClock.now();
diff --git a/src/sot_loader_basic.cpp b/src/sot_loader_basic.cpp
index 1672e81df98c1cbe461750b2232040ee5d02e2d3..6a04bc55b48e98821d9be2eeefb3c11b703892ef 100644
--- a/src/sot_loader_basic.cpp
+++ b/src/sot_loader_basic.cpp
@@ -24,9 +24,15 @@ using namespace std;
 using namespace dynamicgraph::sot;
 namespace po = boost::program_options;
 
-SotLoaderBasic::SotLoaderBasic()
-    : dynamic_graph_stopped_(true), sotRobotControllerLibrary_(0) {
-  nh_ = dynamic_graph_bridge::get_ros_node("SotLoaderBasic");
+SotLoaderBasic::SotLoaderBasic(const std::string &aNodeName)
+    : rclcpp::Node(aNodeName),
+      dynamic_graph_stopped_(true),
+      sotRobotControllerLibrary_(0) {
+  RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge::"),
+              "Beginning of SotLoaderBasic");
+  //  nh_ = dynamic_graph_bridge::get_ros_node("SotLoaderBasic");
+  RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+              "End of SotLoaderBasic");  
 }
 
 SotLoaderBasic::~SotLoaderBasic() {
@@ -35,31 +41,41 @@ SotLoaderBasic::~SotLoaderBasic() {
 
 void SotLoaderBasic::initialize() {}
 
-rclcpp::Node::SharedPtr SotLoaderBasic::returnsNodeHandle() { return nh_; }
+//rclcpp::Node::SharedPtr SotLoaderBasic::returnsNodeHandle() { return nh_; }
 int SotLoaderBasic::initPublication() {
   // Prepare message to be published
+  RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+              "SotLoaderBasic::initPublication - create joint_pub");
+  
   joint_pub_ =
-      nh_->create_publisher<sensor_msgs::msg::JointState>("joint_states", 1);
+      create_publisher<sensor_msgs::msg::JointState>("joint_states", 1);
 
+  RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+              "SotLoaderBasic::initPublication - after create joint_pub");  
   return 0;
 }
 
 void SotLoaderBasic::initializeServices() {
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-              "Ready to start dynamic graph.");
+              "SotLoaderBasic::initializeServices - Ready to start dynamic graph.");
 
   using namespace std::placeholders;
-  service_start_ = nh_->create_service<std_srvs::srv::Empty>(
+  service_start_ = 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.");
 
-  service_stop_ = nh_->create_service<std_srvs::srv::Empty>(
+  service_stop_ = 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.");
 
-  dynamicgraph::parameter_server_read_robot_description(nh_);
+  rclcpp::Node::SharedPtr a_node_ptr(this);
+  dynamicgraph::parameter_server_read_robot_description(a_node_ptr);
 }
 
 void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) {
@@ -70,29 +86,24 @@ int SotLoaderBasic::readSotVectorStateParam() {
   std::map<std::string, int> from_state_name_to_state_vector;
   std::map<std::string, std::string> from_parallel_name_to_state_vector_name;
 
-  if (!nh_) {
-    throw std::logic_error(
-        "SotLoaderBasic::readSotVectorStateParam() nh_ not initialized");
-  }
-
   // It is necessary to declare parameters first
   // before trying to access them.
-  if (not nh_->has_parameter("state_vector_map"))
-    nh_->declare_parameter("state_vector_map", std::vector<std::string>{});
-  if (not nh_->has_parameter("joint_state_parallel"))
-    nh_->declare_parameter("joint_state_parallel", std::vector<std::string>{});
+  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>{});
 
   // Read the state vector of the robot
   // Defines the order in which the actuators are ordered
   try {
     std::string aParameterName("state_vector_map");
-    if (!nh_->get_parameter(aParameterName, stateVectorMap_)) {
+    if (!get_parameter(aParameterName, stateVectorMap_)) {
       logic_error aLogicError(
           "SotLoaderBasic::readSotVectorStateParam : State_vector_map is "
           "empty");
       throw aLogicError;
     }
-    RCLCPP_INFO(nh_->get_logger(), "state_vector_map parameter size %d",
+    RCLCPP_INFO(get_logger(), "state_vector_map parameter size %ld",
                 stateVectorMap_.size());
   } catch (exception& e) {
     std::throw_with_nested(
@@ -108,7 +119,7 @@ int SotLoaderBasic::readSotVectorStateParam() {
 
   std::string prefix("joint_state_parallel");
   std::map<std::string, rclcpp::Parameter> joint_state_parallel;
-  nh_->get_parameters(prefix, joint_state_parallel);
+  get_parameters(prefix, joint_state_parallel);
 
   // Iterates over the map joint_state_parallel
   for (std::map<std::string, rclcpp::Parameter>::iterator it_map_expression =
@@ -172,11 +183,13 @@ int SotLoaderBasic::parseOptions(int argc, char* argv[]) {
   po::notify(vm_);
 
   if (vm_.count("help")) {
-    cout << desc << "\n";
+    RCLCPP_ERROR(rclcpp::get_logger("dynamic_graph_bridge"),
+                 " Help was called \n");
     return -1;
   }
   if (!vm_.count("input-file")) {
-    cout << "No filename specified\n";
+    RCLCPP_ERROR(rclcpp::get_logger("dynamic_graph_bridge"),
+                 "No filename specified\n");
     return -1;
   } else
     dynamicLibraryName_ = vm_["input-file"].as<string>();
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 80349bcdc2ab947614afb7186983170987b3d4bb..b8472eb1a9d197e297e6f087b0294e25eb9f6065 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -16,7 +16,9 @@ if(BUILD_TESTING)
                                                              $ORIGIN)
   target_link_libraries(
     impl_test_sot_mock_device PUBLIC sot-core::sot-core
-                                     dynamic-graph-python::dynamic-graph-python)
+    dynamic-graph-python::dynamic-graph-python
+    ros_bridge
+  )
 
   install(
     TARGETS impl_test_sot_mock_device
@@ -24,21 +26,28 @@ if(BUILD_TESTING)
     DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
 
   file(MAKE_DIRECTORY
-       ${PROJECT_BINARY_DIR}/tests/dynamic_graph/ros/impl_test_sot_mock_device)
-
+    ${PROJECT_BINARY_DIR}/tests/dynamic_graph/ros/tests/impl_test_sot_mock_device)
+  
   dynamic_graph_python_module(
-    "${PYTHON_DIR}/ros/impl_test_sot_mock_device" impl_test_sot_mock_device
+    "${PYTHON_DIR}/ros/tests/impl_test_sot_mock_device" impl_test_sot_mock_device
     dynamic_graph_bridge-impl_test_sot_mock_device-wrap SOURCE_PYTHON_MODULE
     "${CMAKE_CURRENT_SOURCE_DIR}/impl_test_sot_mock_device-python-module-py.cc")
 
   # Library for sot_external_interface
   add_library(impl_test_library SHARED impl_test_sot_external_interface.cpp)
+  set_target_properties(impl_test_library PROPERTIES INSTALL_RPATH
+    $ORIGIN)
 
-  target_include_directories(impl_test_library PUBLIC include)
   target_link_libraries(impl_test_library PUBLIC impl_test_sot_mock_device
                                                  sot-core::sot-core)
   ament_target_dependencies(impl_test_library PUBLIC dynamic_graph_bridge_msgs
-                            rclcpp rcl_interfaces std_srvs)
+    rclcpp rcl_interfaces std_srvs)
+
+  install(
+    TARGETS impl_test_library
+    EXPORT ${TARGETS_EXPORT_NAME}
+    DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
+
   # Executable for SotLoaderBasic test
   add_executable(test_sot_loader_basic test_sot_loader_basic.cpp)
   target_include_directories(
diff --git a/tests/impl_test_sot_mock_device.cpp b/tests/impl_test_sot_mock_device.cpp
index c9e9e9ad7f21dc56d68e001c706f92b5ab65d333..3d85fe286c3e9a9db86455873cb0b884b28bf38c 100644
--- a/tests/impl_test_sot_mock_device.cpp
+++ b/tests/impl_test_sot_mock_device.cpp
@@ -116,9 +116,11 @@ void ImplTestSotMockDevice::setSensorsForce(
     for (int i = 0; i < 4; ++i) {
       sotDEBUG(15) << "Force sensor " << i << std::endl;
       int idx_sensor = map_sot_2_urdf[i];
-      for (int j = 0; j < 6; ++j) {
-        dgforces_(j) = forcesIn[idx_sensor * 6 + j];
-        sotDEBUG(15) << "Force value " << j << ":" << dgforces_(j) << std::endl;
+      for (std::vector<double>::size_type j = 0; j < 6; ++j) {
+        dgforces_(static_cast<Eigen::Index>(j)) =
+            forcesIn[static_cast<std::vector<double>::size_type>(idx_sensor * 6) + j];
+        sotDEBUG(15) << "Force value " << j << ":"
+                     << dgforces_(static_cast<Eigen::Index>(j)) << std::endl;
       }
       forcesSOUT[i]->setConstant(dgforces_);
       forcesSOUT[i]->setTime(t);
@@ -144,7 +146,8 @@ void ImplTestSotMockDevice::setSensorsIMU(
   if (it != SensorsIn.end()) {
     const vector<double>& accelerometer =
         SensorsIn["accelerometer_0"].getValues();
-    for (std::size_t i = 0; i < 3; ++i) accelerometer_(i) = accelerometer[i];
+    for (std::size_t i = 0; i < 3; ++i)
+      accelerometer_(static_cast<Eigen::Index>(i)) = accelerometer[i];
     accelerometerSOUT_.setConstant(accelerometer_);
     accelerometerSOUT_.setTime(t);
   }
@@ -152,7 +155,8 @@ void ImplTestSotMockDevice::setSensorsIMU(
   it = SensorsIn.find("gyrometer_0");
   if (it != SensorsIn.end()) {
     const vector<double>& gyrometer = SensorsIn["gyrometer_0"].getValues();
-    for (std::size_t i = 0; i < 3; ++i) gyrometer_(i) = gyrometer[i];
+    for (std::size_t i = 0; i < 3; ++i)
+      gyrometer_(static_cast<Eigen::Index>(i)) = gyrometer[i];
     gyrometerSOUT_.setConstant(gyrometer_);
     gyrometerSOUT_.setTime(t);
   }
@@ -165,8 +169,8 @@ void ImplTestSotMockDevice::setSensorsEncoders(
   it = SensorsIn.find("motor-angles");
   if (it != SensorsIn.end()) {
     const vector<double>& anglesIn = it->second.getValues();
-    dgRobotState_.resize(anglesIn.size() + 6);
-    motor_angles_.resize(anglesIn.size());
+    dgRobotState_.resize(static_cast<Eigen::Index>(anglesIn.size()) + 6);
+    motor_angles_.resize(static_cast<Eigen::Index>(anglesIn.size()));
     for (unsigned i = 0; i < 6; ++i) dgRobotState_(i) = 0.;
     for (unsigned i = 0; i < anglesIn.size(); ++i) {
       dgRobotState_(i + 6) = anglesIn[i];
@@ -181,7 +185,7 @@ void ImplTestSotMockDevice::setSensorsEncoders(
   it = SensorsIn.find("joint-angles");
   if (it != SensorsIn.end()) {
     const vector<double>& joint_anglesIn = it->second.getValues();
-    joint_angles_.resize(joint_anglesIn.size());
+    joint_angles_.resize(static_cast<Eigen::Index>(joint_anglesIn.size()));
     for (unsigned i = 0; i < joint_anglesIn.size(); ++i)
       joint_angles_(i) = joint_anglesIn[i];
     joint_anglesSOUT_.setConstant(joint_angles_);
@@ -196,9 +200,9 @@ void ImplTestSotMockDevice::setSensorsVelocities(
   it = SensorsIn.find("velocities");
   if (it != SensorsIn.end()) {
     const vector<double>& velocitiesIn = it->second.getValues();
-    dgRobotVelocity_.resize(velocitiesIn.size() + 6);
+    dgRobotVelocity_.resize(static_cast<Eigen::Index>(velocitiesIn.size()) + 6);
     for (unsigned i = 0; i < 6; ++i) dgRobotVelocity_(i) = 0.;
-    for (unsigned i = 0; i < velocitiesIn.size(); ++i) {
+    for (unsigned i = 0; i < static_cast<Eigen::Index>(velocitiesIn.size()); ++i) {
       dgRobotVelocity_(i + 6) = velocitiesIn[i];
     }
     robotVelocity_.setConstant(dgRobotVelocity_);
@@ -212,8 +216,9 @@ void ImplTestSotMockDevice::setSensorsTorquesCurrents(
   it = SensorsIn.find("torques");
   if (it != SensorsIn.end()) {
     const std::vector<double>& torques = SensorsIn["torques"].getValues();
-    torques_.resize(torques.size());
-    for (std::size_t i = 0; i < torques.size(); ++i) torques_(i) = torques[i];
+    torques_.resize(static_cast<Eigen::Index>(torques.size()));
+    for (std::size_t i = 0; i < torques.size(); ++i)
+      torques_(static_cast<Eigen::Index>(i)) = torques[i];
     pseudoTorqueSOUT.setConstant(torques_);
     pseudoTorqueSOUT.setTime(t);
   }
@@ -221,9 +226,9 @@ void ImplTestSotMockDevice::setSensorsTorquesCurrents(
   it = SensorsIn.find("currents");
   if (it != SensorsIn.end()) {
     const std::vector<double>& currents = SensorsIn["currents"].getValues();
-    currents_.resize(currents.size());
+    currents_.resize(static_cast<Eigen::Index>(currents.size()));
     for (std::size_t i = 0; i < currents.size(); ++i)
-      currents_(i) = currents[i];
+      currents_(static_cast<Eigen::Index>(i)) = currents[i];
     currentsSOUT_.setConstant(currents_);
     currentsSOUT_.setTime(t);
   }
@@ -235,8 +240,9 @@ void ImplTestSotMockDevice::setSensorsGains(
   it = SensorsIn.find("p_gains");
   if (it != SensorsIn.end()) {
     const std::vector<double>& p_gains = SensorsIn["p_gains"].getValues();
-    p_gains_.resize(p_gains.size());
-    for (std::size_t i = 0; i < p_gains.size(); ++i) p_gains_(i) = p_gains[i];
+    p_gains_.resize(static_cast<Eigen::Index>(p_gains.size()));
+    for (std::size_t i = 0; i < p_gains.size(); ++i)
+      p_gains_(static_cast<Eigen::Index>(i)) = p_gains[i];
     p_gainsSOUT_.setConstant(p_gains_);
     p_gainsSOUT_.setTime(t);
   }
@@ -244,8 +250,9 @@ void ImplTestSotMockDevice::setSensorsGains(
   it = SensorsIn.find("d_gains");
   if (it != SensorsIn.end()) {
     const std::vector<double>& d_gains = SensorsIn["d_gains"].getValues();
-    d_gains_.resize(d_gains.size());
-    for (std::size_t i = 0; i < d_gains.size(); ++i) d_gains_(i) = d_gains[i];
+    d_gains_.resize(static_cast<Eigen::Index>(d_gains.size()));
+    for (std::size_t i = 0; i < d_gains.size(); ++i)
+      d_gains_(static_cast<Eigen::Index>(i)) = d_gains[i];
     d_gainsSOUT_.setConstant(d_gains_);
     d_gainsSOUT_.setTime(t);
   }
@@ -290,8 +297,8 @@ void ImplTestSotMockDevice::getControl(
   ODEBUG5FULL("start");
   sotDEBUGIN(25);
   vector<double> anglesOut, velocityOut;
-  anglesOut.resize(state_.size());
-  velocityOut.resize(state_.size());
+  anglesOut.resize(static_cast<std::vector<double>::size_type>(state_.size()));
+  velocityOut.resize(static_cast<std::vector<double>::size_type>(state_.size()));
 
   // Integrate control
   sotDEBUG(25) << "state = " << state_ << std::endl;
@@ -310,8 +317,8 @@ void ImplTestSotMockDevice::getControl(
   // warning: we make here the asumption that the control signal contains the
   // velocity of the freeflyer joint. This may change in the future.
   if ((int)anglesOut.size() != state_.size() - 6) {
-    anglesOut.resize(state_.size() - 6);
-    velocityOut.resize(state_.size() - 6);
+    anglesOut.resize(static_cast<std::vector<double>::size_type>(state_.size() - 6));
+    velocityOut.resize(static_cast<std::vector<double>::size_type>(state_.size() - 6));
   }
 
   dg::sigtime_t time = controlSIN.getTime();
diff --git a/tests/launch/launching_test_sot_loader_basic.py b/tests/launch/launching_test_sot_loader_basic.py
index 13b5ad1037c27ff098230c4a5271a6723dd0fc5c..7ef3489c96c6eebef9a87316e5d0a1023f645116 100644
--- a/tests/launch/launching_test_sot_loader_basic.py
+++ b/tests/launch/launching_test_sot_loader_basic.py
@@ -55,6 +55,7 @@ def generate_test_description():
             {"state_vector_map": ["joint1", "joint2"]},
             {"robot_description": robot_description_content},
         ],
+        arguments=['--ros-args', '--log-level', 'INFO']
     )
 
     return (
@@ -74,7 +75,7 @@ class TestSotLoaderBasic(unittest.TestCase):
 
     def test_termination(self, terminating_process, proc_info):
         """Runs the generate_test_description() function."""
-        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/test_common.hpp b/tests/test_common.hpp
index 5d2243977d089512b1928f1bc3564d88189d4124..c3310b989e9efd4446147d04e727784a3d061568 100644
--- a/tests/test_common.hpp
+++ b/tests/test_common.hpp
@@ -40,7 +40,7 @@ void start_dg_ros_service() {
       std::make_shared<std_srvs::srv::Empty::Request>();
   std::shared_future<std_srvs::srv::Empty::Response::SharedPtr> response;
   // Call the service.
-  response = client->async_send_request(request);
+  response = client->async_send_request(request).future.share();
   // Wait for the answer.
   ASSERT_TRUE(response.valid());
   // wait_for_response(response);
@@ -61,7 +61,7 @@ void stop_dg_ros_service() {
       std::make_shared<std_srvs::srv::Empty::Request>();
   std::shared_future<std_srvs::srv::Empty::Response::SharedPtr> response;
   // Call the service.
-  response = client->async_send_request(request);
+  response = client->async_send_request(request).future.share();
   // Wait for the answer.
   ASSERT_TRUE(response.valid());
   // wait_for_response(response);
@@ -91,7 +91,7 @@ void start_run_python_command_ros_service(const std::string& cmd,
       response;
   request->input = cmd;
   // Call the service.
-  response = client->async_send_request(request);
+  response = client->async_send_request(request).future.share();
   // Wait for the answer.
   ASSERT_TRUE(response.valid());
   // wait_for_response(response);
@@ -123,7 +123,7 @@ void start_run_python_script_ros_service(const std::string& file_name,
       response;
   request->input = file_name;
   // Call the service.
-  response = client->async_send_request(request);
+  response = client->async_send_request(request).future.share();
   // Wait for the answer.
   ASSERT_TRUE(response.valid());
   // wait_for_response(response);
diff --git a/tests/test_sot_loader_basic.cpp b/tests/test_sot_loader_basic.cpp
index c6990c6b790e5edd5918fed61ec6aea1e29fb24a..a052403ce8bb377d18a83dc719854f52248a0844 100644
--- a/tests/test_sot_loader_basic.cpp
+++ b/tests/test_sot_loader_basic.cpp
@@ -1,16 +1,33 @@
-
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Wdeprecated-copy"
 #include <gmock/gmock.h>
+#pragma clang diagnostic pop
 
 #include "dynamic_graph_bridge/ros.hpp"
 #include "dynamic_graph_bridge/sot_loader_basic.hh"
 
+
+namespace test_sot_loader_basic {
+int l_argc;
+char** l_argv;
+}
+
 class MockSotLoaderBasicTest : public ::testing::Test {
  public:
   class MockSotLoaderBasic : public SotLoaderBasic {
    public:
+    MockSotLoaderBasic(const std::string &aNodeName=
+                       std::string("MockSotLoaderBasic")) :
+        SotLoaderBasic(aNodeName)
+    {}
     virtual ~MockSotLoaderBasic() {}
 
     void checkStateVectorMap() {
+      // This test makes sure that the class test_sot_loader_basic is able to
+      // read parameters from the ROS-2 space.
+      // It is suppose to read :
+      // state_vector_map and populate member stateVectorMap_
+      // in this test we should have two joints: [ "joint1", "joint2"]
       readSotVectorStateParam();
       ASSERT_EQ(static_cast<int>(stateVectorMap_.size()), 2);
       ASSERT_EQ(nbOfJoints_, 2);
@@ -26,12 +43,14 @@ class MockSotLoaderBasicTest : public ::testing::Test {
       char argv1[30] = "mocktest";
       char *argv[3];
       argv[0] = argv1;
+      // Checking that parseOptions returns -1
       EXPECT_EQ(parseOptions(argc, argv), -1);
 
       // Test help call
       argc = 2;
       char argv2[30] = "--help";
       argv[1] = argv2;
+      // Checking that parseOptions returns -1
       EXPECT_EQ(parseOptions(argc, argv), -1);
 
       // Test input file
@@ -50,11 +69,11 @@ class MockSotLoaderBasicTest : public ::testing::Test {
       char argv1[30] = "mocktest";
       argv[0] = argv1;
       parseOptions(argc, argv);
+      initPublication();
+      EXPECT_TRUE(joint_pub_ != 0);
       initializeServices();
       EXPECT_TRUE(service_start_ != 0);
       EXPECT_TRUE(service_stop_ != 0);
-      initPublication();
-      EXPECT_TRUE(joint_pub_ != 0);
     }
 
     void testJointStatesPublication() {
@@ -122,16 +141,39 @@ class MockSotLoaderBasicTest : public ::testing::Test {
 
  public:
   MockSotLoaderBasic *mockSotLoaderBasic_ptr_;
+  unsigned int nb_tests_;
 
+  MockSotLoaderBasicTest() {
+    nb_tests_ = 0;
+  }
+      
+  // For the set of tests coded in this file.
+  static void SetUpTestCase() {
+    
+    rclcpp::init(test_sot_loader_basic::l_argc,
+                 test_sot_loader_basic::l_argv);
+  }
+
+  // For each test specified in this file
+  static void TearDownTestCase() {
+    rclcpp::shutdown();
+  }
+  
+  // For each test specified by TEST_F
   void SetUp() {
-    mockSotLoaderBasic_ptr_ = new MockSotLoaderBasic();
+    std::string aSotLoaderBasicName("MockSotLoaderBasic" +
+                                    std::to_string(nb_tests_));
+    mockSotLoaderBasic_ptr_ = new MockSotLoaderBasic(aSotLoaderBasicName);
     mockSotLoaderBasic_ptr_->initialize();
   }
-
+  
+  // For each test specified by TEST_F
   void TearDown() {
     delete mockSotLoaderBasic_ptr_;
     mockSotLoaderBasic_ptr_ = nullptr;
+    nb_tests_++;
   }
+
 };
 
 TEST_F(MockSotLoaderBasicTest, TestReadingParameters) {
@@ -160,12 +202,10 @@ TEST_F(MockSotLoaderBasicTest, CleanUp) {
 
 int main(int argc, char **argv) {
   ::testing::InitGoogleTest(&argc, argv);
-  rclcpp::init(argc, argv);
-
+  test_sot_loader_basic::l_argc = argc;
+  test_sot_loader_basic::l_argv = argv;
+  
   int r = RUN_ALL_TESTS();
 
-  std::cout << "ros shutdown" << std::endl;
-  rclcpp::shutdown();
-  std::cout << "ros shutdown done" << std::endl;
   return r;
 }