[tools][hardware-control-interface] Implements unit-test for the sot_loader class.

include/sot/core/abstract-sot-external-interface.hh

@@ -65,4 +65,4 @@ createSotExternalInterface_t();
 typedef void destroySotExternalInterface_t(
     dynamicgraph::sot::AbstractSotExternalInterface *);
 
-#endif
+#endif // ABSTRACT_SOT_EXTERNAL_INTERFACE_HH

include/sot/core/sot-loader.hh

@@ -26,6 +26,24 @@
 namespace dynamicgraph {
 namespace sot {
 
+/**
+ * @brief This class is loading the control part of the Stack-Of-Tasks.
+ * - 1/ It loads dynamically the graph interface.
+ * - 2/ It loads the python interpreter.
+ * - 3/ It loads the Device entity C++ pointer inside the python interpreter.
+ * - 4/ It provides the user interface to the graph:
+ *    - 4.1/ starts and stop the graph executtion.
+ *    - 4.2/ run a python command/script inside the embeded python interpreter.
+ *    - 4.3/ execute one iteration of the graph.
+ *
+ * In order to Use this class you need to provide a dynamic library containing
+ * an implementation of the AbstractSotExternalInterface class.
+ *
+ * Then you can either inherite from this class an initialize and use the
+ * sensors_in_ and control_values_ objects.
+ * Or you can create you own outside of this class.
+ * And then use the oneIteration to execute the graph.
+ */
 class SotLoader {
 protected:
   /// \brief Check if the dynamic graph is running or not.
@@ -44,12 +62,16 @@ protected:
   /// \brief Embeded python interpreter.
   python::Interpreter embeded_python_interpreter_;
 
-  /// Map of sensor readings
+  /// \brief Map of sensor readings
   std::map<std::string, SensorValues> sensors_in_;
 
-  /// Map of control values
+  /// \brief Map of control values
   std::map<std::string, ControlValues> control_values_;
 
+  /// \brief Device entity created and loaded, so we deregister it as the Pool
+  /// is not responsible for it's life time.
+  std::string device_name_;
+
 public:
   /// \brief Default constructor.
   SotLoader();
@@ -60,7 +82,7 @@ public:
   int parseOptions(int argc, char *argv[]);
 
   /// \brief Prepare the SoT framework.
-  void initialization();
+  bool initialization();
 
   /// \brief Unload the library which handles the robot device.
   void cleanUp();
@@ -84,17 +106,20 @@ public:
                         std::string &out, std::string &err);
 
   /// \brief Run a python script inside the embeded python interpreter.
-  void runPythonFile(const std::string &ifilename);
+  inline void runPythonFile(std::string ifilename,
+                                       std::string &err) {
+    embeded_python_interpreter_.runPythonFile(ifilename, err);
+  }
 
-  /// \brief Initialize the external interface sensor reading.
-  void setupSensors();
+  /// \brief Run a python script inside the embeded python interpreter.
+  inline void runPythonFile(std::string ifilename) {
+    embeded_python_interpreter_.runPythonFile(ifilename);
+  }
 
   /// \brief Compute one iteration of control.
-  /// Basically calls fillSensors, the SoT and the readControl.
-  void oneIteration();
-
-  /// \brief Load the Device entity in the python global scope.
-  void loadDeviceInPython(const Device &device);
+  /// Basically executes fillSensors, the SoT and the readControl.
+  void oneIteration(std::map<std::string, SensorValues> &sensors_in,
+                    std::map<std::string, ControlValues> &control_values);
 
   /// \brief Load the Device entity in the python global scope.
   void loadDeviceInPython(const std::string &device_name);

src/tools/sot-loader.cpp

@@ -35,6 +35,7 @@ SotLoader::SotLoader() {
   sot_external_interface_ = nullptr;
   sot_dynamic_library_filename_ = "";
   sot_dynamic_library_ = nullptr;
+  device_name_ = "";
 }
 
 SotLoader::~SotLoader() { cleanUp(); }
@@ -66,13 +67,13 @@ int SotLoader::parseOptions(int argc, char *argv[]) {
   return 0;
 }
 
-void SotLoader::initialization() {
+bool SotLoader::initialization() {
   // Load the library containing the AbstractSotExternalInterface.
   sot_dynamic_library_ =
       dlopen(sot_dynamic_library_filename_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
   if (!sot_dynamic_library_) {
     std::cerr << "Cannot load library: " << dlerror() << '\n';
-    return;
+    return false;
   }
 
   // reset errors
@@ -85,13 +86,14 @@ void SotLoader::initialization() {
   const char *dlsym_error = dlerror();
   if (dlsym_error) {
     std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
-    return;
+    return false;
   }
 
   // Create robot-controller
   sot_external_interface_ = createSotExternalInterface();
-  std::cout << "SoT loaded from " << sot_dynamic_library_filename_ << "."
-            << std::endl;
+  assert(sot_external_interface_ && "Fail to create the sotExternalInterface");
+  std::cout << "SoT loaded at address [" << &sot_external_interface_
+            << "] from " << sot_dynamic_library_filename_ << "." << std::endl;
 
   // Init the python interpreter.
   std::string result, out, err;
@@ -119,9 +121,18 @@ void SotLoader::initialization() {
   // Debug print.
   runPythonCommand("print(\"Executing python interpreter prologue... Done\")",
                    result, out, err);
+
+  return true;
 }
 
 void SotLoader::cleanUp() {
+  // Unregister the device first if it exists to avoid a double destruction from
+  // the pool of entity and the class that handle the Device pointer.
+  if(device_name_ != "")
+  {
+    PoolStorage::getInstance()->deregisterEntity(device_name_);
+  }
+
   // We do not destroy the FactoryStorage singleton because the module will not
   // be reloaded at next initialization (because Python C API cannot safely
   // unload a module...).
@@ -155,28 +166,21 @@ void SotLoader::runPythonCommand(const std::string &command,
   embeded_python_interpreter_.python(command, result, out, err);
 }
 
-void SotLoader::runPythonFile(const std::string &ifilename) {
-  embeded_python_interpreter_.runPythonFile(ifilename);
-}
-
-void SotLoader::setupSensors() {
-  sot_external_interface_->setupSetSensors(sensors_in_);
-  sot_external_interface_->getControl(control_values_);
-}
-
-void SotLoader::oneIteration() {
-  try {
-    sot_external_interface_->nominalSetSensors(sensors_in_);
-    sot_external_interface_->getControl(control_values_);
-  } catch (std::exception &) {
-    throw;
+void SotLoader::oneIteration(
+    std::map<std::string, SensorValues> &sensors_in,
+    std::map<std::string, ControlValues> &control_values) {
+  if (!dynamic_graph_stopped_) {
+    try {
+      sot_external_interface_->nominalSetSensors(sensors_in);
+      sot_external_interface_->getControl(control_values);
+    } catch (std::exception &e) {
+      std::cout << "Exception while running the graph:\n"
+                << e.what() << std::endl;
+      throw e;
+    }
   }
 }
 
-void SotLoader::loadDeviceInPython(const Device &device) {
-  loadDeviceInPython(device.getName());
-}
-
 void SotLoader::loadDeviceInPython(const std::string &device_name) {
   std::string result, out, err;
   // Debug print.
@@ -188,12 +192,17 @@ void SotLoader::loadDeviceInPython(const std::string &device_name) {
                    err);
 
   // Get the existing C++ entity pointer in the Python interpreter.
-  runPythonCommand("device_cpp_object = Device(" + device_name + ")",
-                   result, out, err);
+  runPythonCommand("loaded_device_name = \"" + device_name + "\"", result,
+                   out, err);
+  runPythonCommand("device_cpp_object = Device(loaded_device_name)", result,
+                   out, err);
 
   // Debug print.
   runPythonCommand("print(\"Load device from C++ to Python... Done!!\")",
                    result, out, err);
+
+  // strore the device name to unregister it upon cleanup.
+  device_name_ = device_name;
 }
 
 } /* namespace sot */

tests/CMakeLists.txt

@@ -83,6 +83,7 @@ SET(tests
 
   tools/test_boost
   tools/test_device
+  tools/test_sot_loader
   tools/test_mailbox
   tools/test_matrix
   tools/test_robot_utils
@@ -105,8 +106,13 @@ IF(UNIX)
   TARGET_LINK_LIBRARIES(test_abstract_interface PRIVATE
     Boost::program_options
     pluginabstract ${CMAKE_DL_LIBS})
+
+  ADD_LIBRARY(dummy_sot_external_interface SHARED
+    tools/dummy-sot-external-interface.cc)
 ENDIF(UNIX)
 
+
+
 FOREACH(path ${tests})
   GET_FILENAME_COMPONENT(test ${path} NAME)
   ADD_UNIT_TEST(${test} ${path}.cpp)
@@ -114,6 +120,14 @@ FOREACH(path ${tests})
   TARGET_LINK_LIBRARIES(${test} PRIVATE ${PROJECT_NAME}
     Boost::unit_test_framework
     ${TEST_${test}_LIBS} ${TEST_${test}_EXT_LIBS})
+  
+  # add some preprocessor variable
+  TARGET_COMPILE_DEFINITIONS(
+    ${test}
+    PUBLIC LIB_PLUGIN_ABSTRACT_PATH="${CMAKE_BINARY_DIR}/tests/libdummy_sot_external_interface.so"
+           TEST_GOOD_PYTHON_SCRIPT="${PROJECT_SOURCE_DIR}/tests/tools/good_python_script.py"
+           TEST_BAD_PYTHON_SCRIPT="${PROJECT_SOURCE_DIR}/tests/tools/bad_python_script.py"
+  )
 
   IF(UNIX)
     TARGET_LINK_LIBRARIES(${test} PRIVATE ${CMAKE_DL_LIBS})

tests/tools/bad_python_script.py

+a = 1+1
+print(b)
\ No newline at end of file

tests/tools/dummy-sot-external-interface.cc

+/*
+ * Copyright 2011,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ */
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+#include <iostream>
+#include <sot/core/debug.hh>
+
+#include "dummy-sot-external-interface.hh"
+
+using namespace std;
+using namespace dynamicgraph::sot;
+
+void DummySotExternalInterface::setupSetSensors(
+    std::map<std::string, dynamicgraph::sot::SensorValues> &sensorsIn) {
+  nominalSetSensors(sensorsIn);
+  return;
+}
+
+void DummySotExternalInterface::nominalSetSensors(
+    std::map<std::string, dynamicgraph::sot::SensorValues> &/*sensorsIn*/) {
+  return;
+}
+
+void DummySotExternalInterface::cleanupSetSensors(
+    std::map<std::string, dynamicgraph::sot::SensorValues> &sensorsIn) {
+  nominalSetSensors(sensorsIn);
+  return;
+}
+
+void DummySotExternalInterface::getControl(
+    std::map<std::string, dynamicgraph::sot::ControlValues> &controlOut) {
+  controlOut["ctrl_map_name"] = dynamicgraph::sot::ControlValues();
+  controlOut["ctrl_map_name"].setName("ctrl_value_name");
+  controlOut["ctrl_map_name"].setValues(std::vector<double>(5, 3.1415));
+  return;
+}
+
+void DummySotExternalInterface::setSecondOrderIntegration(void) {
+  second_integration_ = true;
+  return;
+}
+
+void DummySotExternalInterface::setNoIntegration(void) {
+  second_integration_ = false;
+  return;
+}
+
+extern "C" {
+dynamicgraph::sot::AbstractSotExternalInterface *createSotExternalInterface() {
+  return new DummySotExternalInterface();
+}
+}
+
+extern "C" {
+void destroySotExternalInterface(
+    dynamicgraph::sot::AbstractSotExternalInterface *p) {
+  delete p;
+}
+}
\ No newline at end of file

tests/tools/dummy-sot-external-interface.hh

+/**
+ * @file abstract-sot-external-interface-tester.hh
+ * @author your name (you@domain.com)
+ * @brief
+ * @version 0.1
+ * @date 2021-11-17
+ *
+ * @copyright Copyright (c) 2021
+ *
+ */
+
+#ifndef ABSTRACT_SOT_EXTERNAL_INTERFACE_TESTER_HH
+#define ABSTRACT_SOT_EXTERNAL_INTERFACE_TESTER_HH
+
+#include <sot/core/abstract-sot-external-interface.hh>
+
+class DummySotExternalInterface
+    : public dynamicgraph::sot::AbstractSotExternalInterface {
+public:
+  DummySotExternalInterface(){};
+
+  virtual ~DummySotExternalInterface(){};
+
+  virtual void setupSetSensors(
+      std::map<std::string, dynamicgraph::sot::SensorValues> &sensorsIn);
+
+  virtual void nominalSetSensors(
+      std::map<std::string, dynamicgraph::sot::SensorValues> &sensorsIn);
+
+  virtual void cleanupSetSensors(
+      std::map<std::string, dynamicgraph::sot::SensorValues> &sensorsIn);
+
+  virtual void getControl(
+      std::map<std::string, dynamicgraph::sot::ControlValues> &controlOut);
+  virtual void setSecondOrderIntegration(void);
+  virtual void setNoIntegration(void);
+
+public:
+  bool second_integration_;
+};
+
+#endif /* ABSTRACT_SOT_EXTERNAL_INTERFACE_TESTER_HH */

tests/tools/good_python_script.py

+a = 1+1
+b = a
+print(b)
\ No newline at end of file

tests/tools/test_sot_loader.cpp

+/*
+ * License BSD3
+ * Copyright 2021,
+ * Maximilien Naveau,
+ *
+ * CNRS
+ *
+ */
+
+#define BOOST_TEST_MODULE test_sot_loader
+#include <boost/test/included/unit_test.hpp>
+
+#include <fstream>
+#include <iostream>
+
+#include <dynamic-graph/entity.h>
+#include <dynamic-graph/factory.h>
+#include <dynamic-graph/pool.h>
+
+#include "sot/core/debug.hh"
+#include "sot/core/device.hh"
+#include "sot/core/sot-loader.hh"
+
+using namespace dynamicgraph;
+using namespace dynamicgraph::sot;
+namespace dg = dynamicgraph;
+
+class TestDevice : public dg::sot::Device {
+public:
+  TestDevice(const std::string &RobotName) : Device(RobotName) {
+    timestep_ = 0.001;
+  }
+  ~TestDevice() {}
+};
+
+struct TestFixture {
+  TestFixture() {
+    asotei_lib_file_ = LIB_PLUGIN_ABSTRACT_PATH;
+    good_python_script_ = TEST_GOOD_PYTHON_SCRIPT;
+    bad_python_script_ = TEST_BAD_PYTHON_SCRIPT;
+  }
+  ~TestFixture() {}
+  /// @brief Path to the AbstractSotExternalInterface test library.
+  std::string asotei_lib_file_;
+  /// @brief Path to a python srcipt to parse without error.
+  std::string good_python_script_;
+  /// @brief Path to a python srcipt to parse with error.
+  std::string bad_python_script_;
+  /// @brief Sensor values container.
+  std::map<std::string, SensorValues> sensors;
+  /// @brief Control values container.
+  std::map<std::string, ControlValues> controls;
+};
+
+BOOST_AUTO_TEST_SUITE(sot_loader_test_suite)
+
+BOOST_FIXTURE_TEST_CASE(test_plugin_existance, TestFixture) {
+  std::ifstream file(asotei_lib_file_);
+  bool file_exists = false;
+  if (file) {
+    file_exists = true;
+  }
+  BOOST_CHECK(file_exists);
+  BOOST_CHECK(asotei_lib_file_ != "");
+}
+
+BOOST_FIXTURE_TEST_CASE(test_default_behavior, TestFixture) {
+  SotLoader sot_loader;
+  sot_loader.initialization();
+  BOOST_CHECK(sot_loader.isDynamicGraphStopped());
+}
+
+BOOST_FIXTURE_TEST_CASE(test_start_stop_dg, TestFixture) {
+  SotLoader sot_loader;
+  sot_loader.initialization();
+  BOOST_CHECK(sot_loader.isDynamicGraphStopped());
+  sot_loader.startDG();
+  BOOST_CHECK(!sot_loader.isDynamicGraphStopped());
+  sot_loader.stopDG();
+  BOOST_CHECK(sot_loader.isDynamicGraphStopped());
+}
+
+BOOST_FIXTURE_TEST_CASE(test_option_parsing_input_file, TestFixture) {
+  SotLoader sot_loader;
+  sot_loader.setDynamicLibraryName(asotei_lib_file_);
+
+  char argv0[] = "test_sot_loader";
+  char argv1[] = "--input-file";
+  char argv2[] = LIB_PLUGIN_ABSTRACT_PATH;
+  char *argv[] = {argv0, argv1, argv2, NULL};
+  sot_loader.parseOptions(3, argv);
+  BOOST_CHECK(sot_loader.initialization());
+}
+
+BOOST_FIXTURE_TEST_CASE(test_option_parsing_sot_dynamic_library, TestFixture) {
+  SotLoader sot_loader;
+  sot_loader.setDynamicLibraryName(asotei_lib_file_);
+
+  char argv0[] = "test_sot_loader";
+  char argv1[] = "--sot-dynamic-library";
+  char argv2[] = LIB_PLUGIN_ABSTRACT_PATH;
+  char *argv[] = {argv0, argv1, argv2, NULL};
+  sot_loader.parseOptions(3, argv);
+  BOOST_CHECK(sot_loader.initialization());
+}
+
+BOOST_FIXTURE_TEST_CASE(test_sot_loader_set_dynamic_library_name, TestFixture) {
+  SotLoader sot_loader;
+  sot_loader.setDynamicLibraryName(asotei_lib_file_);
+  BOOST_CHECK(sot_loader.initialization());
+}
+
+BOOST_FIXTURE_TEST_CASE(test_sot_loader_one_iteration, TestFixture) {
+  std::vector<double> controls_values;
+  SotLoader sot_loader;
+  sot_loader.setDynamicLibraryName(asotei_lib_file_);
+  sot_loader.initialization();
+  // Without running the graph:
+  sot_loader.oneIteration(sensors, controls);
+  BOOST_CHECK(controls.find("ctrl_map_name") == controls.end());
+  // With the graph running:
+  sot_loader.startDG();
+  std::cout << "running the graph" << std::endl;
+  sot_loader.oneIteration(sensors, controls);
+  std::cout << "running the graph ... done" << std::endl;
+  controls_values = controls["ctrl_map_name"].getValues();
+  BOOST_CHECK_EQUAL(controls_values.size(), 5);
+  for (auto value : controls_values) {
+    BOOST_CHECK_EQUAL(value, 3.1415);
+  }
+}
+
+BOOST_FIXTURE_TEST_CASE(test_cleanup_no_init, TestFixture) {
+  SotLoader sot_loader;
+  sot_loader.cleanUp();
+}
+
+BOOST_FIXTURE_TEST_CASE(test_cleanup_init, TestFixture) {
+  SotLoader sot_loader;
+  sot_loader.setDynamicLibraryName(asotei_lib_file_);
+  sot_loader.initialization();
+  sot_loader.cleanUp();
+}
+
+BOOST_FIXTURE_TEST_CASE(test_run_python_command, TestFixture) {
+  SotLoader sot_loader;
+  sot_loader.setDynamicLibraryName(asotei_lib_file_);
+  sot_loader.initialization();
+  std::string res, out, err;
+  sot_loader.runPythonCommand("1+1", res, out, err);
+  BOOST_CHECK_EQUAL(res, "2");
+  BOOST_CHECK_EQUAL(out, "");
+  BOOST_CHECK_EQUAL(err, "");
+}
+
+BOOST_FIXTURE_TEST_CASE(test_run_python_command_error, TestFixture) {
+  SotLoader sot_loader;
+  sot_loader.setDynamicLibraryName(asotei_lib_file_);
+  sot_loader.initialization();
+  std::string res, out, err;
+  sot_loader.runPythonCommand("print(a)", res, out, err);
+  std::cout << std::quoted(err) << std::endl;
+  BOOST_CHECK_EQUAL(res, "");
+  BOOST_CHECK_EQUAL(out, "");
+  BOOST_CHECK_EQUAL(err, "Traceback (most recent call last):\n"
+                         "  File \"<string>\", line 1, in <module>\n"
+                         "NameError: name 'a' is not defined\n");
+}
+
+BOOST_FIXTURE_TEST_CASE(test_run_python_scripts, TestFixture) {
+  SotLoader sot_loader;
+  sot_loader.setDynamicLibraryName(asotei_lib_file_);
+  sot_loader.initialization();
+  std::string err;
+  sot_loader.runPythonFile(good_python_script_, err);
+  BOOST_CHECK_EQUAL(err, "");
+}
+
+BOOST_FIXTURE_TEST_CASE(test_run_python_scripts_failure, TestFixture) {
+  SotLoader sot_loader;
+  sot_loader.setDynamicLibraryName(asotei_lib_file_);
+  sot_loader.initialization();
+  std::string err;
+  sot_loader.runPythonFile(bad_python_script_, err);
+  BOOST_CHECK_EQUAL(err, "Traceback (most recent call last):\n"
+                         "  File \"" +
+                             bad_python_script_ +
+                             "\", line 2, in <module>\n"
+                             "    print(b)\n"
+                             "NameError: name 'b' is not defined\n");
+}
+
+BOOST_FIXTURE_TEST_CASE(test_load_device_in_python, TestFixture) {
+  SotLoader sot_loader;
+  sot_loader.setDynamicLibraryName(asotei_lib_file_);
+  sot_loader.initialization();
+  Device device("device_name");
+  sot_loader.loadDeviceInPython(device.getName());
+  std::string res, out, err;
+  sot_loader.runPythonCommand("print(device_cpp_object.name)", res, out, err);
+  BOOST_CHECK_EQUAL(res, "None");
+  BOOST_CHECK_EQUAL(out, "device_name\n");
+  BOOST_CHECK_EQUAL(err, "");
+}
+
+BOOST_AUTO_TEST_SUITE_END()