Commit 69f72072 authored by Maximilien Naveau's avatar Maximilien Naveau
Browse files

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

parent 5487dc25
Pipeline #16998 failed with stage
in 13 minutes and 16 seconds
......@@ -65,4 +65,4 @@ createSotExternalInterface_t();
typedef void destroySotExternalInterface_t(
dynamicgraph::sot::AbstractSotExternalInterface *);
#endif
#endif // ABSTRACT_SOT_EXTERNAL_INTERFACE_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);
......
......@@ -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 */
......
......@@ -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})
......
a = 1+1
print(b)
\ No newline at end of file
/*
* 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
/**
* @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 */
a = 1+1
b = a
print(b)
\ No newline at end of file
/*
* 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()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment