From e73fe413c94b73dd5338dd2998e8ea81b42ba90b Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Sat, 25 Nov 2023 09:17:13 +0100
Subject: [PATCH 01/24] Make possible to change Node name.

---
 include/dynamic_graph_bridge/sot_loader.hh | 2 +-
 src/sot_loader.cpp                         | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
index 3b0ed29..3413449 100644
--- a/include/dynamic_graph_bridge/sot_loader.hh
+++ b/include/dynamic_graph_bridge/sot_loader.hh
@@ -83,7 +83,7 @@ class SotLoader : public SotLoaderBasic {
   geometry_msgs::msg::TransformStamped freeFlyerPose_;
 
  public:
-  SotLoader();
+  SotLoader(const std::string &aNodeName=std::string("SotLoader"));
   virtual ~SotLoader();
 
   // \brief Create a thread for ROS and start the control loop.
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 6f56066..a88f9e2 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_(),

From 35b419bf16b317be6b0dc0d38ac85f1cf59b481a Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Wed, 29 Nov 2023 02:10:18 +0100
Subject: [PATCH 02/24] Remove headers using pinocchio

---
 include/dynamic_graph_bridge/sot_loader.hh       | 3 ---
 include/dynamic_graph_bridge/sot_loader_basic.hh | 3 ---
 src/sot_loader_basic.cpp                         | 3 ---
 3 files changed, 9 deletions(-)

diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
index 3413449..83cf89b 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>
diff --git a/include/dynamic_graph_bridge/sot_loader_basic.hh b/include/dynamic_graph_bridge/sot_loader_basic.hh
index 35ac763..42a3b27 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>
diff --git a/src/sot_loader_basic.cpp b/src/sot_loader_basic.cpp
index d6023bd..93aee03 100644
--- a/src/sot_loader_basic.cpp
+++ b/src/sot_loader_basic.cpp
@@ -19,9 +19,6 @@
 #include <sot/core/robot-utils.hh>
 #include <stdexcept>
 
-#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/parsers/urdf.hpp"
-
 // POSIX.1-2001
 #include <dlfcn.h>
 

From 34f6c7aae9d4359661c76b933b3fbb26dbe69f37 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Wed, 29 Nov 2023 02:16:24 +0100
Subject: [PATCH 03/24] Prepare test_sot_loader to run scripts in embedded
 python. Prepare reception of ROS-2 topic from the SoT.

---
 tests/CMakeLists.txt      | 12 ++++++---
 tests/test_sot_loader.cpp | 53 ++++++++++++++++++++++++++++++++++++---
 2 files changed, 58 insertions(+), 7 deletions(-)

diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 2a7b4dd..c4e0d47 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,6 @@ 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/")
   endmacro(create_ros_bridge_unittest test_name)
 
   # C++ unit-tests.
diff --git a/tests/test_sot_loader.cpp b/tests/test_sot_loader.cpp
index 59ca291..dcaff40 100644
--- a/tests/test_sot_loader.cpp
+++ b/tests/test_sot_loader.cpp
@@ -1,15 +1,39 @@
-
+#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.hh"
+#include "dynamic_graph_bridge_msgs/msg/vector.hpp"
+#include "test_common.hpp"
+
+namespace test_sot_loader {
+int l_argc;
+char** l_argv;
+}
+
+namespace dg = dynamicgraph;
 
 class MockSotLoaderTest : public ::testing::Test {
  public:
   class MockSotLoader : public SotLoader {
    public:
+    rclcpp::Subscription<dynamic_graph_bridge_msgs::msg::Vector>::SharedPtr
+    subscription_;
+
     ~MockSotLoader() {}
 
+    void topic_callback(const dynamic_graph_bridge_msgs::msg::Vector::SharedPtr msg) const {
+      auto lsize=msg->data.size();
+    }
+
+    void subscribe_to_a_topic() {
+      subscription_ = create_subscription<dynamic_graph_bridge_msgs::msg::Vector>(
+          "control_ros", 1, std::bind(&MockSotLoader::topic_callback, this,
+                                      std::placeholders::_1));
+    }
+
     void generateEvents() {
       usleep(20000);
       std::cerr << "Start Dynamic Graph " << std::endl;
@@ -33,6 +57,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);
@@ -54,6 +79,14 @@ class MockSotLoaderTest : public ::testing::Test {
       // Start the thread generating events.
       std::thread local_events(&MockSotLoader::generateEvents, this);
 
+      // Create and call the clients.
+      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, result);
+
       // Wait for each threads.
       SotLoader::lthread_join();  // Wait 100 ms
       local_events.join();
@@ -63,6 +96,18 @@ class MockSotLoaderTest : public ::testing::Test {
  public:
   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();
@@ -80,10 +125,12 @@ TEST_F(MockSotLoaderTest, TestLoadController) {
 
 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;
 }

From b1afbcb7b5d10c380b03a60c77b912d1d69dbd79 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Wed, 29 Nov 2023 02:18:20 +0100
Subject: [PATCH 04/24] Increasing timeout

---
 tests/test_ros_init.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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));
 }
 
 /**

From 3ba10213c245e524cbae6df1a7fd9d4d6bcf84dc Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Wed, 29 Nov 2023 02:19:34 +0100
Subject: [PATCH 05/24] Services are inside /dynamic_graph_bridge namespace.

---
 src/ros_python_interpreter_server.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/ros_python_interpreter_server.cpp b/src/ros_python_interpreter_server.cpp
index 1162626..ca96144 100644
--- a/src/ros_python_interpreter_server.cpp
+++ b/src/ros_python_interpreter_server.cpp
@@ -30,13 +30,13 @@ void RosPythonInterpreterServer::start_ros_service() {
       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);
 
   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);
 }
 
 void RosPythonInterpreterServer::runCommandCallback(

From 8a53dd9562cf4457c8fd4886bd2762498d7a531c Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Wed, 29 Nov 2023 02:20:20 +0100
Subject: [PATCH 06/24] [test] Minor modifications on test_ros_interpreter.

---
 tests/test_ros_interpreter.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/tests/test_ros_interpreter.cpp b/tests/test_ros_interpreter.cpp
index 5ad5523..1129125 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,7 @@ 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, "");
+
 }

From 75ef4f5ac0d32b6fdb82666f715e2fc1f095d000 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Wed, 29 Nov 2023 02:21:43 +0100
Subject: [PATCH 07/24] [tests] Add call to services for
 sot_external_interfaces.

---
 tests/impl_test_sot_external_interface.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/tests/impl_test_sot_external_interface.cpp b/tests/impl_test_sot_external_interface.cpp
index e162b67..607ca5e 100644
--- a/tests/impl_test_sot_external_interface.cpp
+++ b/tests/impl_test_sot_external_interface.cpp
@@ -55,6 +55,7 @@ void ImplTestSotExternalInterface::init() {
   double ts = 0.001;
 
   py_inter_ptr->declare_parameter<double>("/sot_controller/dt", ts);
+  py_interpreter_srv_->start_ros_service();
   device_->timeStep(ts);
 }
 

From 876bfcc14858eeafac141396298fcc8283ca4466 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Wed, 29 Nov 2023 02:22:20 +0100
Subject: [PATCH 08/24] [tests] Fix mistake on wrong module name.

---
 tests/impl_test_sot_mock_device-python-module-py.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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> >();

From f60963f619e6faed986a202e2c599f4a0ab2691b Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Wed, 29 Nov 2023 02:23:07 +0100
Subject: [PATCH 09/24] [tests] Increase timeout for tests in launch file.

---
 tests/launch/launching_test_sot_loader.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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()

From a48149f106135ff1671882d5deb3741c95c3910c Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Sat, 2 Dec 2023 03:13:47 +0100
Subject: [PATCH 10/24] Add display the list of nodes and the number of
 threads.

---
 include/dynamic_graph_bridge/ros.hpp |  5 +++++
 src/ros.cpp                          | 25 +++++++++++++++++++++++++
 2 files changed, 30 insertions(+)

diff --git a/include/dynamic_graph_bridge/ros.hpp b/include/dynamic_graph_bridge/ros.hpp
index bbf1e0e..5340598 100644
--- a/include/dynamic_graph_bridge/ros.hpp
+++ b/include/dynamic_graph_bridge/ros.hpp
@@ -67,6 +67,11 @@ typedef std_srvs::srv::Empty EmptyServiceType;
  */
 RosNodePtr get_ros_node(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/src/ros.cpp b/src/ros.cpp
index c306efc..dd07121 100644
--- a/src/ros.cpp
+++ b/src/ros.cpp
@@ -131,6 +131,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
@@ -204,6 +211,7 @@ std::string executable_name() {
 #endif
 }
 
+
 /**
  * @brief Private function that allow us to initialize ROS only once.
  */
@@ -273,6 +281,23 @@ 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",
+                ros_node_it->second->get_namespace(),
+                ros_node_it->second->get_name());
+    ros_node_it++;
+  }
+
+}
+
 void ros_clean() {
   ros_stop_spinning();
   GlobalListOfRosNodeType::iterator ros_node_it =

From a8803aef1ff6a451a1a9e40ca56fe5196e519cae Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Sat, 2 Dec 2023 03:18:39 +0100
Subject: [PATCH 11/24] Reuse the MultiThreaded::Executor to handle ROS Nodes.

---
 include/dynamic_graph_bridge/sot_loader.hh    |  2 +-
 .../dynamic_graph_bridge/sot_loader_basic.hh  |  8 ++-
 src/sot_loader.cpp                            | 26 ++++----
 src/sot_loader_basic.cpp                      | 62 +++++++++++--------
 4 files changed, 54 insertions(+), 44 deletions(-)

diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
index 83cf89b..9d4070a 100644
--- a/include/dynamic_graph_bridge/sot_loader.hh
+++ b/include/dynamic_graph_bridge/sot_loader.hh
@@ -80,7 +80,7 @@ class SotLoader : public SotLoaderBasic {
   geometry_msgs::msg::TransformStamped freeFlyerPose_;
 
  public:
-  SotLoader(const std::string &aNodeName=std::string("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 42a3b27..8be36bf 100644
--- a/include/dynamic_graph_bridge/sot_loader_basic.hh
+++ b/include/dynamic_graph_bridge/sot_loader_basic.hh
@@ -38,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_;
 
@@ -80,8 +83,9 @@ class SotLoaderBasic : public rclcpp::Node {
   // Ordered list of joint names describing the robot state.
   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/sot_loader.cpp b/src/sot_loader.cpp
index a88f9e2..9db6ded 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -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,7 +196,6 @@ 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() {
@@ -230,11 +226,11 @@ 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 +246,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 93aee03..26ecf1c 100644
--- a/src/sot_loader_basic.cpp
+++ b/src/sot_loader_basic.cpp
@@ -27,10 +27,13 @@ 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),
       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"),
@@ -48,7 +51,7 @@ int SotLoaderBasic::initPublication() {
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
               "SotLoaderBasic::initPublication - create joint_pub");
 
-  joint_pub_ =
+  joint_pub_ = ros_node_->
       create_publisher<sensor_msgs::msg::JointState>("joint_states", 1);
 
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
@@ -62,19 +65,21 @@ void SotLoaderBasic::initializeServices() {
       "SotLoaderBasic::initializeServices - Ready to start dynamic graph.");
 
   using namespace std::placeholders;
-  service_start_ = create_service<std_srvs::srv::Empty>(
-      "start_dynamic_graph",
-      std::bind(&SotLoaderBasic::start_dg, this, std::placeholders::_1,
-                std::placeholders::_2));
+  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>(
-      "stop_dynamic_graph",
-      std::bind(&SotLoaderBasic::stop_dg, this, std::placeholders::_1,
-                std::placeholders::_2));
+  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();
 }
@@ -89,22 +94,22 @@ 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"));
@@ -119,7 +124,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 =
@@ -202,7 +207,9 @@ 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;
   }
 
@@ -215,14 +222,17 @@ 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() {
@@ -269,13 +279,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");

From 09feb19f1227baf55d028777d9bac0d4c87e571b Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Sat, 2 Dec 2023 03:25:24 +0100
Subject: [PATCH 12/24] [cmake] Add SHARED library to sot_loader.[cmake] Add
 SHARED library to sot_loader.[cmake] Add SHARED library to sot_loader.[cmake]
 Add SHARED library to sot_loader.[cmake] Add SHARED library to sot_loader.

---
 src/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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)

From a6981b900f0d8a3a88761c4e869d04ebf4a0240e Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Sat, 2 Dec 2023 03:30:15 +0100
Subject: [PATCH 13/24] [tests] Restructure test_sot_loader.

---
 tests/CMakeLists.txt                       |   4 +
 tests/impl_test_sot_external_interface.cpp |  17 ++-
 tests/test_sot_loader.cpp                  | 139 ++++++++++++++++++---
 3 files changed, 140 insertions(+), 20 deletions(-)

diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index c4e0d47..184907b 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -115,6 +115,10 @@ if(BUILD_TESTING)
     ament_target_dependencies(${current_test_name} dynamic_graph_bridge_msgs
                               rclcpp rcl_interfaces std_srvs)
 
+    # 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 607ca5e..34b0100 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);
 
@@ -52,11 +53,23 @@ void ImplTestSotExternalInterface::init() {
 
   RosNodePtr py_inter_ptr = get_ros_node("python_interpreter");
   // Set the control time step parameter to 0.001
-  double ts = 0.001;
+  double ts = 0.01;
 
+  // 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.
   py_interpreter_srv_->start_ros_service();
+
+  // 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(
@@ -66,7 +79,7 @@ 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/test_sot_loader.cpp b/tests/test_sot_loader.cpp
index dcaff40..12b32e1 100644
--- a/tests/test_sot_loader.cpp
+++ b/tests/test_sot_loader.cpp
@@ -3,10 +3,14 @@
 #include <gmock/gmock.h>
 #pragma clang diagnostic pop
 
+using namespace std::chrono_literals;
+
+#include <rclcpp/rclcpp.hpp>
+
 #include "dynamic_graph_bridge/ros.hpp"
 #include "dynamic_graph_bridge/sot_loader.hh"
 #include "dynamic_graph_bridge_msgs/msg/vector.hpp"
-#include "test_common.hpp"
+#include "dynamic_graph_bridge/ros_python_interpreter_server.hpp"
 
 namespace test_sot_loader {
 int l_argc;
@@ -21,29 +25,131 @@ class MockSotLoaderTest : public ::testing::Test {
    public:
     rclcpp::Subscription<dynamic_graph_bridge_msgs::msg::Vector>::SharedPtr
     subscription_;
+    bool service_done_;
+    std::string node_name_;
+    std::string response_dg_python_result_;
 
-    ~MockSotLoader() {}
+    MockSotLoader():
+        node_name_("unittest_sot_loader") {}
 
-    void topic_callback(const dynamic_graph_bridge_msgs::msg::Vector::SharedPtr msg) const {
-      auto lsize=msg->data.size();
+    ~MockSotLoader() {
+      service_done_ = false;
+      response_dg_python_result_ = "";
     }
 
+    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() {
-      subscription_ = create_subscription<dynamic_graph_bridge_msgs::msg::Vector>(
-          "control_ros", 1, std::bind(&MockSotLoader::topic_callback, this,
-                                      std::placeholders::_1));
+      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));
+    }
+
+    // This method is to receive the result of client request to run_python_file
+    //
+    void response_dg_python(
+        const rclcpp::Client<dynamic_graph_bridge_msgs::srv::RunPythonFile>::SharedFuture
+        future) {
+      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"), "response_dg_python:");
+      auto status = future.wait_for(500ms);
+      if (status == std::future_status::ready) {
+        // uncomment below line if using Empty() message
+        // RCLCPP_INFO(this->get_logger(), "Result: success");
+        // comment below line if using Empty() message
+        RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+                    "response_dg_python - Result: %s",
+                    future.get()->result.c_str());
+        service_done_ = true;
+        response_dg_python_result_ = future.get()->result;
+      } else {
+        RCLCPP_INFO(rclcpp::get_logger("dynmic_graph_bridge"),
+                    "response_dg_python - Service In-Progress...");
+      }
+    }
+
+    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.
+      auto client = dynamic_graph_bridge::get_ros_node(node_name_)->
+          create_client<dynamic_graph_bridge_msgs::srv::RunPythonFile>(
+              service_name);
+      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,
+                                     std::bind(&MockSotLoader::response_dg_python,
+                                               this,
+                                               std::placeholders::_1));
+      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"),
+                  "Get the answer");
+
+    }
+
+    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;
@@ -79,16 +185,11 @@ class MockSotLoaderTest : public ::testing::Test {
       // Start the thread generating events.
       std::thread local_events(&MockSotLoader::generateEvents, this);
 
-      // Create and call the clients.
-      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, result);
+      // 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();
     }
   };
@@ -111,11 +212,13 @@ class MockSotLoaderTest : public ::testing::Test {
   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();
   }
 };
 

From a53b72192c900aa1bcfc6e60f78839bab4c3f60f Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Sat, 2 Dec 2023 03:30:55 +0100
Subject: [PATCH 14/24] Add debug RCLCPP info.

---
 src/ros_python_interpreter_server.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/src/ros_python_interpreter_server.cpp b/src/ros_python_interpreter_server.cpp
index ca96144..9f93ada 100644
--- a/src/ros_python_interpreter_server.cpp
+++ b/src/ros_python_interpreter_server.cpp
@@ -31,12 +31,18 @@ void RosPythonInterpreterServer::start_ros_service() {
                 std::placeholders::_1, std::placeholders::_2);
   run_python_command_srv_ = ros_node_->create_service<RunPythonCommandSrvType>(
       "/dynamic_graph_bridge/run_python_command", runCommandCb);
+  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>(
       "/dynamic_graph_bridge/run_python_file", runPythonFileCb);
+
+  RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+              "RosPythonInterpreterServer::start_ros_service - run_python_file...");
+
 }
 
 void RosPythonInterpreterServer::runCommandCallback(
@@ -61,7 +67,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,

From ce39b175e9f0ac70df15808413c8432ae2991250 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Sat, 2 Dec 2023 03:31:28 +0100
Subject: [PATCH 15/24] [tests] Modify simple_ros_publish.py to increase the
 test range.

---
 tests/python_scripts/simple_ros_publish.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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"

From 3e1929c0a8dee302237b6e8a1aff60a918cd53dc Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@laas.fr>
Date: Sat, 2 Dec 2023 03:39:24 +0100
Subject: [PATCH 16/24] [pre-commit] Beautification

---
 include/dynamic_graph_bridge/ros.hpp          |   1 -
 include/dynamic_graph_bridge/sot_loader.hh    |   2 +-
 .../dynamic_graph_bridge/sot_loader_basic.hh  |   6 +-
 scripts/remote_python_client.py               |   2 +-
 src/ros.cpp                                   |   4 +-
 src/ros_python_interpreter_server.cpp         |  11 +-
 src/sot_loader.cpp                            |   6 +-
 src/sot_loader_basic.cpp                      |  37 +++----
 tests/CMakeLists.txt                          |   7 +-
 tests/impl_test_sot_external_interface.cpp    |   3 +-
 tests/test_ros_interpreter.cpp                |   1 -
 tests/test_sot_loader.cpp                     | 100 ++++++++----------
 12 files changed, 84 insertions(+), 96 deletions(-)

diff --git a/include/dynamic_graph_bridge/ros.hpp b/include/dynamic_graph_bridge/ros.hpp
index 5340598..b11acb3 100644
--- a/include/dynamic_graph_bridge/ros.hpp
+++ b/include/dynamic_graph_bridge/ros.hpp
@@ -67,7 +67,6 @@ typedef std_srvs::srv::Empty EmptyServiceType;
  */
 RosNodePtr get_ros_node(std::string node_name);
 
-
 size_t ros_executor_get_nb_threads();
 
 void ros_display_list_of_nodes();
diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
index 9d4070a..f844ba6 100644
--- a/include/dynamic_graph_bridge/sot_loader.hh
+++ b/include/dynamic_graph_bridge/sot_loader.hh
@@ -80,7 +80,7 @@ class SotLoader : public SotLoaderBasic {
   geometry_msgs::msg::TransformStamped freeFlyerPose_;
 
  public:
-  SotLoader(const std::string &aNodeName=std::string("sot_loader"));
+  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 8be36bf..76187f7 100644
--- a/include/dynamic_graph_bridge/sot_loader_basic.hh
+++ b/include/dynamic_graph_bridge/sot_loader_basic.hh
@@ -38,7 +38,7 @@
 namespace po = boost::program_options;
 namespace dgs = dynamicgraph::sot;
 
-class SotLoaderBasic  {
+class SotLoaderBasic {
  protected:
   // RosNode
   dynamic_graph_bridge::RosNodePtr ros_node_;
@@ -83,9 +83,9 @@ class SotLoaderBasic  {
   // Ordered list of joint names describing the robot state.
   std::vector<std::string> stateVectorMap_;
 
-
  public:
-  SotLoaderBasic(const std::string& aNodeName = std::string("sot_loader_basic"));
+  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/scripts/remote_python_client.py b/scripts/remote_python_client.py
index 2fb0432..e3c7023 100755
--- a/scripts/remote_python_client.py
+++ b/scripts/remote_python_client.py
@@ -24,10 +24,10 @@
 from pathlib import Path
 
 import rclpy
+from dynamic_graph_bridge_cpp_bindings import RosPythonInterpreterClient
 
 # Used to connect to ROS services
 from dynamic_graph_bridge.ros.dgcompleter import DGCompleter
-from dynamic_graph_bridge_cpp_bindings import RosPythonInterpreterClient
 
 
 def signal_handler(sig, frame):
diff --git a/src/ros.cpp b/src/ros.cpp
index dd07121..0f8f215 100644
--- a/src/ros.cpp
+++ b/src/ros.cpp
@@ -211,7 +211,6 @@ std::string executable_name() {
 #endif
 }
 
-
 /**
  * @brief Private function that allow us to initialize ROS only once.
  */
@@ -288,14 +287,13 @@ size_t ros_executor_get_nb_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()) {
+  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",
                 ros_node_it->second->get_namespace(),
                 ros_node_it->second->get_name());
     ros_node_it++;
   }
-
 }
 
 void ros_clean() {
diff --git a/src/ros_python_interpreter_server.cpp b/src/ros_python_interpreter_server.cpp
index 9f93ada..ce92f7d 100644
--- a/src/ros_python_interpreter_server.cpp
+++ b/src/ros_python_interpreter_server.cpp
@@ -31,8 +31,9 @@ void RosPythonInterpreterServer::start_ros_service() {
                 std::placeholders::_1, std::placeholders::_2);
   run_python_command_srv_ = ros_node_->create_service<RunPythonCommandSrvType>(
       "/dynamic_graph_bridge/run_python_command", runCommandCb);
-  RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-              "RosPythonInterpreterServer::start_ros_service - run_python_command...");
+  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,
@@ -40,9 +41,9 @@ void RosPythonInterpreterServer::start_ros_service() {
   run_python_file_srv_ = ros_node_->create_service<RunPythonFileSrvType>(
       "/dynamic_graph_bridge/run_python_file", runPythonFileCb);
 
-  RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-              "RosPythonInterpreterServer::start_ros_service - run_python_file...");
-
+  RCLCPP_INFO(
+      rclcpp::get_logger("dynamic_graph_bridge"),
+      "RosPythonInterpreterServer::start_ros_service - run_python_file...");
 }
 
 void RosPythonInterpreterServer::runCommandCallback(
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 9db6ded..ae0f363 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -226,8 +226,10 @@ void SotLoader::workThreadLoader() {
   double periodd;
 
   /// Declare parameters
-  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);
+  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);
 
   //
   ros_node_->get_parameter_or("dt", periodd, 0.001);
diff --git a/src/sot_loader_basic.cpp b/src/sot_loader_basic.cpp
index 26ecf1c..a8f3593 100644
--- a/src/sot_loader_basic.cpp
+++ b/src/sot_loader_basic.cpp
@@ -31,7 +31,6 @@ SotLoaderBasic::SotLoaderBasic(const std::string& aNodeName)
       dynamic_graph_stopped_(true),
       sotController_(NULL),
       sotRobotControllerLibrary_(0) {
-
   ros_node_ = dynamic_graph_bridge::get_ros_node(aNodeName);
 
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge::"),
@@ -51,8 +50,8 @@ int SotLoaderBasic::initPublication() {
   RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
               "SotLoaderBasic::initPublication - create joint_pub");
 
-  joint_pub_ = ros_node_->
-      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 +64,17 @@ void SotLoaderBasic::initializeServices() {
       "SotLoaderBasic::initializeServices - Ready to start dynamic graph.");
 
   using namespace std::placeholders;
-  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));
+  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 - start_dynamic_graph.");
 
-  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));
+  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 - stop_dynamic_graph.");
 
@@ -95,9 +92,11 @@ int SotLoaderBasic::readSotVectorStateParam() {
   // It is necessary to declare parameters first
   // before trying to access them.
   if (not ros_node_->has_parameter("state_vector_map"))
-    ros_node_->declare_parameter("state_vector_map", std::vector<std::string>{});
+    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>{});
+    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
@@ -208,8 +207,7 @@ void SotLoaderBasic::loadController() {
       dlopen(dynamicLibraryName_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
   if (!sotRobotControllerLibrary_) {
     RCLCPP_ERROR(rclcpp::get_logger("dynamic_graph_bridge"),
-                 "Cannot load library: %s",
-                 dlerror());
+                 "Cannot load library: %s", dlerror());
     return;
   }
 
@@ -223,9 +221,8 @@ void SotLoaderBasic::loadController() {
   const char* dlsym_error = dlerror();
   if (dlsym_error) {
     RCLCPP_ERROR(rclcpp::get_logger("dynamic_graph_bridge"),
-                 "Cannot load symbol create: %s from %s",
-                 dlsym_error,
-                 dynamicLibraryName_.c_str() );
+                 "Cannot load symbol create: %s from %s", dlsym_error,
+                 dynamicLibraryName_.c_str());
     return;
   }
 
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 184907b..aab0fcf 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -67,8 +67,8 @@ if(BUILD_TESTING)
     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/")
+    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)
@@ -116,8 +116,7 @@ if(BUILD_TESTING)
                               rclcpp rcl_interfaces std_srvs)
 
     # Install tests
-    install(TARGETS test_${test_name}
-      DESTINATION lib/${PROJECT_NAME})
+    install(TARGETS test_${test_name} DESTINATION lib/${PROJECT_NAME})
 
   endmacro(create_ros_bridge_unittest test_name)
 
diff --git a/tests/impl_test_sot_external_interface.cpp b/tests/impl_test_sot_external_interface.cpp
index 34b0100..c285411 100644
--- a/tests/impl_test_sot_external_interface.cpp
+++ b/tests/impl_test_sot_external_interface.cpp
@@ -79,7 +79,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/test_ros_interpreter.cpp b/tests/test_ros_interpreter.cpp
index 1129125..10091b3 100644
--- a/tests/test_ros_interpreter.cpp
+++ b/tests/test_ros_interpreter.cpp
@@ -286,5 +286,4 @@ TEST_F(TestRosInterpreter, test_call_run_script_ros_publish) {
 
   /* Tests the result. */
   ASSERT_EQ(result, "");
-
 }
diff --git a/tests/test_sot_loader.cpp b/tests/test_sot_loader.cpp
index 12b32e1..3170c6d 100644
--- a/tests/test_sot_loader.cpp
+++ b/tests/test_sot_loader.cpp
@@ -8,14 +8,14 @@ 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"
-#include "dynamic_graph_bridge/ros_python_interpreter_server.hpp"
 
 namespace test_sot_loader {
 int l_argc;
 char** l_argv;
-}
+}  // namespace test_sot_loader
 
 namespace dg = dynamicgraph;
 
@@ -24,42 +24,45 @@ class MockSotLoaderTest : public ::testing::Test {
   class MockSotLoader : public SotLoader {
    public:
     rclcpp::Subscription<dynamic_graph_bridge_msgs::msg::Vector>::SharedPtr
-    subscription_;
+        subscription_;
     bool service_done_;
     std::string node_name_;
     std::string response_dg_python_result_;
 
-    MockSotLoader():
-        node_name_("unittest_sot_loader") {}
+    MockSotLoader() : node_name_("unittest_sot_loader") {}
 
     ~MockSotLoader() {
       service_done_ = false;
       response_dg_python_result_ = "";
     }
 
-    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;
+    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).
+    // 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() {
-      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_ =
+          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));
     }
 
     // This method is to receive the result of client request to run_python_file
     //
     void response_dg_python(
-        const rclcpp::Client<dynamic_graph_bridge_msgs::srv::RunPythonFile>::SharedFuture
-        future) {
-      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"), "response_dg_python:");
+        const rclcpp::Client<dynamic_graph_bridge_msgs::srv::RunPythonFile>::
+            SharedFuture future) {
+      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+                  "response_dg_python:");
       auto status = future.wait_for(500ms);
       if (status == std::future_status::ready) {
         // uncomment below line if using Empty() message
@@ -80,34 +83,30 @@ class MockSotLoaderTest : public ::testing::Test {
       // Service name.
       std::string service_name = "/dynamic_graph_bridge/run_python_file";
       // Create a client from a temporary node.
-      auto client = dynamic_graph_bridge::get_ros_node(node_name_)->
-          create_client<dynamic_graph_bridge_msgs::srv::RunPythonFile>(
-              service_name);
+      auto client =
+          dynamic_graph_bridge::get_ros_node(node_name_)
+              ->create_client<dynamic_graph_bridge_msgs::srv::RunPythonFile>(
+                  service_name);
       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>();
+      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,
-                                     std::bind(&MockSotLoader::response_dg_python,
-                                               this,
-                                               std::placeholders::_1));
+      auto response = client->async_send_request(
+          request, std::bind(&MockSotLoader::response_dg_python, this,
+                             std::placeholders::_1));
       RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-                  "Send request to service %s - Waiting",
-                  service_name.c_str());
+                  "Send request to service %s - Waiting", service_name.c_str());
       response.wait();
-      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-                  "Get the answer");
-
+      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"), "Get the answer");
     }
 
-    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
+    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";
@@ -119,8 +118,7 @@ class MockSotLoaderTest : public ::testing::Test {
     }
 
     void local_ros_spin() {
-      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-                  "local_ros_spin");
+      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"), "local_ros_spin");
       dynamic_graph_bridge::ros_spin();
     }
 
@@ -137,23 +135,22 @@ class MockSotLoaderTest : public ::testing::Test {
       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_);
+      dynamic_graph_bridge::RosNodePtr ros_node =
+          dynamic_graph_bridge::get_ros_node(node_name_);
       bool simple_service_exists = false;
 
-      usleep(30720000); // Wait 30s
+      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";
@@ -195,19 +192,15 @@ class MockSotLoaderTest : public ::testing::Test {
   };
 
  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);
+    rclcpp::init(test_sot_loader::l_argc, test_sot_loader::l_argv);
   }
 
   // For each test specified in this file
-  static void TearDownTestCase() {
-    rclcpp::shutdown();
-  }
+  static void TearDownTestCase() { rclcpp::shutdown(); }
 
   void SetUp() {
     mockSotLoader_ptr_ = new MockSotLoader();
@@ -226,13 +219,12 @@ TEST_F(MockSotLoaderTest, TestLoadController) {
   mockSotLoader_ptr_->testLoadController();
 }
 
-int main(int argc, char **argv) {
+int main(int argc, char** argv) {
   ::testing::InitGoogleTest(&argc, argv);
 
   test_sot_loader::l_argc = argc;
   test_sot_loader::l_argv = argv;
 
-
   int r = RUN_ALL_TESTS();
 
   return r;

From 7e2a495900d7a5072a793362ad9672c1a863f732 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Sun, 3 Dec 2023 21:17:32 +0100
Subject: [PATCH 17/24] [README.md] Add option for python standard layout
 installation.

---
 README.md | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

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:

From baa5b68a052c1db932436f9ccb1776d88c7dd23c Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Sun, 3 Dec 2023 21:23:42 +0100
Subject: [PATCH 18/24] Modifies the ROS Node map in a map of tuple which
 includes: - The ROS Node - A related callback group with reentrant type.

---
 include/dynamic_graph_bridge/ros.hpp | 23 +++++++++++++---
 src/ros.cpp                          | 39 ++++++++++++++++++----------
 2 files changed, 44 insertions(+), 18 deletions(-)

diff --git a/include/dynamic_graph_bridge/ros.hpp b/include/dynamic_graph_bridge/ros.hpp
index b11acb3..547761b 100644
--- a/include/dynamic_graph_bridge/ros.hpp
+++ b/include/dynamic_graph_bridge/ros.hpp
@@ -60,13 +60,28 @@ 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();
diff --git a/src/ros.cpp b/src/ros.cpp
index 0f8f215..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();
@@ -234,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;
@@ -248,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 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 GLOBAL_LIST_OF_ROS_NODE[node_name];
+  return std::get<1>(GLOBAL_LIST_OF_ROS_NODE[node_name]);
 }
 
 void ros_add_node_to_executor(const std::string& node_name) {
@@ -290,8 +301,8 @@ void ros_display_list_of_nodes() {
   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",
-                ros_node_it->second->get_namespace(),
-                ros_node_it->second->get_name());
+                std::get<0>(ros_node_it->second)->get_namespace(),
+                std::get<0>(ros_node_it->second)->get_name());
     ros_node_it++;
   }
 }

From f968fbed2f32c7b17bfa315bc168cf428cd9457d Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Sun, 3 Dec 2023 21:45:32 +0100
Subject: [PATCH 19/24] [ros_python_interpreter] Add the possibility to get the
 callback_group of reentrant type.

---
 .../dynamic_graph_bridge/ros_python_interpreter_server.hpp | 5 ++++-
 src/ros_python_interpreter_server.cpp                      | 7 ++++---
 2 files changed, 8 insertions(+), 4 deletions(-)

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/src/ros_python_interpreter_server.cpp b/src/ros_python_interpreter_server.cpp
index ce92f7d..272e5d4 100644
--- a/src/ros_python_interpreter_server.cpp
+++ b/src/ros_python_interpreter_server.cpp
@@ -25,12 +25,13 @@ 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>(
-      "/dynamic_graph_bridge/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...");
@@ -39,7 +40,7 @@ void RosPythonInterpreterServer::start_ros_service() {
       std::bind(&RosPythonInterpreterServer::runPythonFileCallback, this,
                 std::placeholders::_1, std::placeholders::_2);
   run_python_file_srv_ = ros_node_->create_service<RunPythonFileSrvType>(
-      "/dynamic_graph_bridge/run_python_file", runPythonFileCb);
+      "/dynamic_graph_bridge/run_python_file", runPythonFileCb, qos, group);
 
   RCLCPP_INFO(
       rclcpp::get_logger("dynamic_graph_bridge"),

From eed8db226dca6eddb183225fd583f8070d46b585 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Sun, 3 Dec 2023 21:48:25 +0100
Subject: [PATCH 20/24] [sot_loader_basic] Add definition of
 BOOST_MPL_LIMIT_VECTOR_SIZE because of pinocchio through robot-utils.hh

---
 src/sot_loader_basic.cpp | 17 ++++++++++++++---
 1 file changed, 14 insertions(+), 3 deletions(-)

diff --git a/src/sot_loader_basic.cpp b/src/sot_loader_basic.cpp
index a8f3593..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,6 +28,8 @@
 #include <sot/core/robot-utils.hh>
 #include <stdexcept>
 
+#include "dynamic_graph_bridge/sot_loader_basic.hh"
+
 // POSIX.1-2001
 #include <dlfcn.h>
 
@@ -29,7 +40,7 @@ namespace po = boost::program_options;
 SotLoaderBasic::SotLoaderBasic(const std::string& aNodeName)
     : ros_node_(nullptr),
       dynamic_graph_stopped_(true),
-      sotController_(NULL),
+      sotController_(nullptr),
       sotRobotControllerLibrary_(0) {
   ros_node_ = dynamic_graph_bridge::get_ros_node(aNodeName);
 

From 84de1b7c349b4fed5824d735dd7b378cd0bc3181 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Sun, 3 Dec 2023 21:50:03 +0100
Subject: [PATCH 21/24] [sot_loader] Check if sot_Controller is NULL.

---
 src/sot_loader.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index ae0f363..7bd1178 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -199,6 +199,7 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
 }
 
 void SotLoader::setup() {
+  if (sotController_ == NULL) return;
   fillSensors(sensorsIn_);
   sotController_->setupSetSensors(sensorsIn_);
   sotController_->getControl(controlValues_);
@@ -206,6 +207,8 @@ void SotLoader::setup() {
 }
 
 void SotLoader::oneIteration() {
+  if (sotController_ == NULL) return;
+
   fillSensors(sensorsIn_);
   try {
     sotController_->nominalSetSensors(sensorsIn_);

From 8e76a12499442fabc3c976bfab9e06a36c3c29c8 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Sun, 3 Dec 2023 21:51:17 +0100
Subject: [PATCH 22/24] [impl_test_sot_external_interface] Set again 1 KHz Use
 the callback group from the node python_interpreter.

---
 tests/impl_test_sot_external_interface.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/tests/impl_test_sot_external_interface.cpp b/tests/impl_test_sot_external_interface.cpp
index c285411..59fa5fd 100644
--- a/tests/impl_test_sot_external_interface.cpp
+++ b/tests/impl_test_sot_external_interface.cpp
@@ -53,12 +53,16 @@ void ImplTestSotExternalInterface::init() {
 
   RosNodePtr py_inter_ptr = get_ros_node("python_interpreter");
   // Set the control time step parameter to 0.001
-  double ts = 0.01;
+  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.
-  py_interpreter_srv_->start_ros_service();
+
+  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.

From d50cdf914be51a66d51810579741ff05d27d0e94 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Sun, 3 Dec 2023 21:55:15 +0100
Subject: [PATCH 23/24] [tests/test_sot_loader] Makes the test of sending a
 script working. The catch is to have the same node, with reentrant type for
 the server and the client when both are loaded in the same process.

---
 tests/test_sot_loader.cpp | 57 +++++++++++++++------------------------
 1 file changed, 21 insertions(+), 36 deletions(-)

diff --git a/tests/test_sot_loader.cpp b/tests/test_sot_loader.cpp
index 3170c6d..f3c95f9 100644
--- a/tests/test_sot_loader.cpp
+++ b/tests/test_sot_loader.cpp
@@ -3,6 +3,8 @@
 #include <gmock/gmock.h>
 #pragma clang diagnostic pop
 
+#include <chrono>
+
 using namespace std::chrono_literals;
 
 #include <rclcpp/rclcpp.hpp>
@@ -27,15 +29,17 @@ class MockSotLoaderTest : public ::testing::Test {
         subscription_;
     bool service_done_;
     std::string node_name_;
-    std::string response_dg_python_result_;
 
-    MockSotLoader() : node_name_("unittest_sot_loader") {}
+    rclcpp::CallbackGroup::SharedPtr reentrant_cb_group_;
 
-    ~MockSotLoader() {
-      service_done_ = false;
-      response_dg_python_result_ = "";
+    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;
@@ -48,45 +52,27 @@ class MockSotLoaderTest : public ::testing::Test {
     // 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));
-    }
-
-    // This method is to receive the result of client request to run_python_file
-    //
-    void response_dg_python(
-        const rclcpp::Client<dynamic_graph_bridge_msgs::srv::RunPythonFile>::
-            SharedFuture future) {
-      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-                  "response_dg_python:");
-      auto status = future.wait_for(500ms);
-      if (status == std::future_status::ready) {
-        // uncomment below line if using Empty() message
-        // RCLCPP_INFO(this->get_logger(), "Result: success");
-        // comment below line if using Empty() message
-        RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
-                    "response_dg_python - Result: %s",
-                    future.get()->result.c_str());
-        service_done_ = true;
-        response_dg_python_result_ = future.get()->result;
-      } else {
-        RCLCPP_INFO(rclcpp::get_logger("dynmic_graph_bridge"),
-                    "response_dg_python - Service In-Progress...");
-      }
+                            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(node_name_)
+          dynamic_graph_bridge::get_ros_node("python_interpreter")
               ->create_client<dynamic_graph_bridge_msgs::srv::RunPythonFile>(
-                  service_name);
+                  service_name, rclcpp::ServicesQoS(), local_cb_group);
       ASSERT_TRUE(client->wait_for_service(1s));
 
       // Fill the command message.
@@ -95,13 +81,13 @@ class MockSotLoaderTest : public ::testing::Test {
               dynamic_graph_bridge_msgs::srv::RunPythonFile::Request>();
       request->input = file_name;
       // Call the service.
-      auto response = client->async_send_request(
-          request, std::bind(&MockSotLoader::response_dg_python, this,
-                             std::placeholders::_1));
+      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"), "Get the answer");
+      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>>&
@@ -178,7 +164,6 @@ class MockSotLoaderTest : public ::testing::Test {
 
       // Start the control loop thread.
       startControlLoop();
-
       // Start the thread generating events.
       std::thread local_events(&MockSotLoader::generateEvents, this);
 

From ab0eeac8aa7d76a79761f0c41db6856e9f3d95fb Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Sun, 3 Dec 2023 20:58:21 +0000
Subject: [PATCH 24/24] [pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
---
 scripts/remote_python_client.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/scripts/remote_python_client.py b/scripts/remote_python_client.py
index e3c7023..2fb0432 100755
--- a/scripts/remote_python_client.py
+++ b/scripts/remote_python_client.py
@@ -24,10 +24,10 @@
 from pathlib import Path
 
 import rclpy
-from dynamic_graph_bridge_cpp_bindings import RosPythonInterpreterClient
 
 # Used to connect to ROS services
 from dynamic_graph_bridge.ros.dgcompleter import DGCompleter
+from dynamic_graph_bridge_cpp_bindings import RosPythonInterpreterClient
 
 
 def signal_handler(sig, frame):