Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/tools/sot-loader.cpp	Lines:	79	90	87.8 %
Date:	2023-03-13 12:09:37	Branches:	94	194	48.5 %


/*
 * Copyright 2016,
 * Olivier Stasse,
 *
 * CNRS
 *
 */
/* -------------------------------------------------------------------------- */
/* --- INCLUDES ------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */

// POSIX.1-2001
#include <dlfcn.h>

// C++ includes
#include <iostream>
#include <sstream>

// Boost includes
#include <boost/program_options.hpp>

// Dynamic Graph includes
#include <dynamic-graph/pool.h>

// Local includes
#include <sot/core/sot-loader.hh>

namespace po = boost::program_options;

namespace dynamicgraph {
namespace sot {

SotLoader::SotLoader() {
  dynamic_graph_stopped_ = true;
  sot_external_interface_ = nullptr;
  sot_dynamic_library_filename_ = "";
  sot_dynamic_library_ = nullptr;
  device_name_ = "";
}

SotLoader::~SotLoader() { cleanUp(); }

int SotLoader::parseOptions(int argc, char *argv[]) {
  po::options_description desc("Allowed options");
  desc.add_options()("help", "produce help message")(
      "sot-dynamic-library", po::value<std::string>(), "Library to load");
  desc.add_options()("help", "produce help message")(
      "input-file", po::value<std::string>(), "Library to load");

  // Input variable map from the command line (int argc, char* argv[]).
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << desc << "\n";
    return -1;
  }
  if (vm.count("sot-dynamic-library")) {
    sot_dynamic_library_filename_ = vm["sot-dynamic-library"].as<std::string>();
  } else if (vm.count("input-file")) {
    sot_dynamic_library_filename_ = vm["input-file"].as<std::string>();
  } else {
    std::cout << "No filename specified\n";
    return -1;
  }
  return 0;
}

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 false;
  }

  // reset errors
  dlerror();

  // Load the symbols.
  createSotExternalInterface_t *createSotExternalInterface =
      reinterpret_cast<createSotExternalInterface_t *>(reinterpret_cast<long>(
          dlsym(sot_dynamic_library_, "createSotExternalInterface")));
  const char *dlsym_error = dlerror();
  if (dlsym_error) {
    std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
    return false;
  }

  // Create robot-controller
  sot_external_interface_ = createSotExternalInterface();
  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;
  // Debug print.
  runPythonCommand("print(\"Executing python interpreter prologue...\")",
                   result, out, err);
  // make sure that the current environment variable are setup in the current
  // python interpreter.
  runPythonCommand("import sys, os", result, out, err);
  runPythonCommand("print(\"python version:\", sys.version)", result, out, err);
  runPythonCommand("pythonpath = os.environ.get('PYTHONPATH', '')", result, out,
                   err);
  runPythonCommand("path = []", result, out, err);
  runPythonCommand(
      "for p in pythonpath.split(':'):\n"
      "  if p not in sys.path:\n"
      "    path.append(p)",
      result, out, err);
  runPythonCommand("path.extend(sys.path)", result, out, err);
  runPythonCommand("sys.path = path", result, out, err);
  // used to be able to invoke rospy
  runPythonCommand(
      "if not hasattr(sys, \'argv\'):\n"
      "    sys.argv  = ['sot']",
      result, out, err);
  // help setting signals
  runPythonCommand("import numpy as np", result, out, err);
  // 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...).
  // SignalCaster singleton could probably be destroyed.
  dynamicgraph::PoolStorage::destroy();

  // Load the symbols.
  if (sot_dynamic_library_ != nullptr && sot_external_interface_ != nullptr) {
    destroySotExternalInterface_t *destroySotExternalInterface =
        reinterpret_cast<destroySotExternalInterface_t *>(
            reinterpret_cast<long>(
                dlsym(sot_dynamic_library_, "destroySotExternalInterface")));
    const char *dlsym_error = dlerror();
    if (dlsym_error) {
      std::cerr << "Cannot load symbol destroy: " << dlsym_error << '\n';
      return;
    }

    destroySotExternalInterface(sot_external_interface_);
    sot_external_interface_ = nullptr;

    /// Uncount the number of access to this library.
    dlclose(sot_dynamic_library_);
    sot_dynamic_library_ = nullptr;
  }
}

void SotLoader::runPythonCommand(const std::string &command,
                                 std::string &result, std::string &out,
                                 std::string &err) {
  embeded_python_interpreter_.python(command, result, out, err);
}

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 std::string &device_name) {
  std::string result, out, err;
  // Debug print.
  runPythonCommand("print(\"Load device from C++ to Python...\")", result, out,
                   err);

  // Import the Device entity declaration
  runPythonCommand("from dynamic_graph.sot.core import Device", result, out,
                   err);

  // Get the existing C++ entity pointer in the Python interpreter.
  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 */
} /* namespace dynamicgraph */


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2016,
3			* Olivier Stasse,
4			*
5			* CNRS
6			*
7			*/
8			/* -------------------------------------------------------------------------- */
9			/* --- INCLUDES ------------------------------------------------------------- */
10			/* -------------------------------------------------------------------------- */
11
12			// POSIX.1-2001
13			#include <dlfcn.h>
14
15			// C++ includes
16			#include <iostream>
17			#include <sstream>
18
19			// Boost includes
20			#include <boost/program_options.hpp>
21
22			// Dynamic Graph includes
23			#include <dynamic-graph/pool.h>
24
25			// Local includes
26			#include <sot/core/sot-loader.hh>
27
28			namespace po = boost::program_options;
29
30			namespace dynamicgraph {
31			namespace sot {
32
33	✓✗	13	SotLoader::SotLoader() {
34		13	dynamic_graph_stopped_ = true;
35		13	sot_external_interface_ = nullptr;
36	✓✗	13	sot_dynamic_library_filename_ = "";
37		13	sot_dynamic_library_ = nullptr;
38	✓✗	13	device_name_ = "";
39		13	}
40
41		13	SotLoader::~SotLoader() { cleanUp(); }
42
43		2	int SotLoader::parseOptions(int argc, char *argv[]) {
44	✓✗✓✗	6	po::options_description desc("Allowed options");
45	✓✗✓✗	2	desc.add_options()("help", "produce help message")(
46	✓✗✓✗	2	"sot-dynamic-library", po::value<std::string>(), "Library to load");
47	✓✗✓✗	2	desc.add_options()("help", "produce help message")(
48	✓✗✓✗	2	"input-file", po::value<std::string>(), "Library to load");
49
50			// Input variable map from the command line (int argc, char* argv[]).
51	✓✗	4	po::variables_map vm;
52	✓✗✓✗	2	po::store(po::parse_command_line(argc, argv, desc), vm);
53	✓✗	2	po::notify(vm);
54
55	✓✗✓✗ ✗✓	2	if (vm.count("help")) {
56			std::cout << desc << "\n";
57			return -1;
58			}
59	✓✗✓✗ ✓✓	2	if (vm.count("sot-dynamic-library")) {
60	✓✗✓✗ ✓✗✓✗	1	sot_dynamic_library_filename_ = vm["sot-dynamic-library"].as<std::string>();
61	✓✗✓✗ ✓✗	1	} else if (vm.count("input-file")) {
62	✓✗✓✗ ✓✗✓✗	1	sot_dynamic_library_filename_ = vm["input-file"].as<std::string>();
63			} else {
64			std::cout << "No filename specified\n";
65			return -1;
66			}
67		2	return 0;
68			}
69
70		12	bool SotLoader::initialization() {
71			// Load the library containing the AbstractSotExternalInterface.
72		12	sot_dynamic_library_ =
73		12	dlopen(sot_dynamic_library_filename_.c_str(), RTLD_LAZY \| RTLD_GLOBAL);
74	✗✓	12	if (!sot_dynamic_library_) {
75			std::cerr << "Cannot load library: " << dlerror() << '\n';
76			return false;
77			}
78
79			// reset errors
80		12	dlerror();
81
82			// Load the symbols.
83			createSotExternalInterface_t *createSotExternalInterface =
84			reinterpret_cast<createSotExternalInterface_t *>(reinterpret_cast<long>(
85		12	dlsym(sot_dynamic_library_, "createSotExternalInterface")));
86		12	const char *dlsym_error = dlerror();
87	✓✓	12	if (dlsym_error) {
88	✓✗✓✗ ✓✗	2	std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
89		2	return false;
90			}
91
92			// Create robot-controller
93	✓✗	10	sot_external_interface_ = createSotExternalInterface();
94	✗✓	10	assert(sot_external_interface_ && "Fail to create the sotExternalInterface");
95	✓✗✓✗	10	std::cout << "SoT loaded at address [" << &sot_external_interface_
96	✓✗✓✗ ✓✗✓✗	10	<< "] from " << sot_dynamic_library_filename_ << "." << std::endl;
97
98			// Init the python interpreter.
99		20	std::string result, out, err;
100			// Debug print.
101	✓✗✓✗	10	runPythonCommand("print(\"Executing python interpreter prologue...\")",
102			result, out, err);
103			// make sure that the current environment variable are setup in the current
104			// python interpreter.
105	✓✗✓✗	10	runPythonCommand("import sys, os", result, out, err);
106	✓✗✓✗	10	runPythonCommand("print(\"python version:\", sys.version)", result, out, err);
107	✓✗✓✗	10	runPythonCommand("pythonpath = os.environ.get('PYTHONPATH', '')", result, out,
108			err);
109	✓✗✓✗	10	runPythonCommand("path = []", result, out, err);
110	✓✗✓✗	10	runPythonCommand(
111			"for p in pythonpath.split(':'):\n"
112			" if p not in sys.path:\n"
113			" path.append(p)",
114			result, out, err);
115	✓✗✓✗	10	runPythonCommand("path.extend(sys.path)", result, out, err);
116	✓✗✓✗	10	runPythonCommand("sys.path = path", result, out, err);
117			// used to be able to invoke rospy
118	✓✗✓✗	10	runPythonCommand(
119			"if not hasattr(sys, \'argv\'):\n"
120			" sys.argv = ['sot']",
121			result, out, err);
122			// help setting signals
123	✓✗✓✗	10	runPythonCommand("import numpy as np", result, out, err);
124			// Debug print.
125	✓✗✓✗	10	runPythonCommand("print(\"Executing python interpreter prologue... Done\")",
126			result, out, err);
127
128		10	return true;
129			}
130
131		15	void SotLoader::cleanUp() {
132			// Unregister the device first if it exists to avoid a double destruction from
133			// the pool of entity and the class that handle the Device pointer.
134	✓✓	15	if (device_name_ != "") {
135		1	PoolStorage::getInstance()->deregisterEntity(device_name_);
136			}
137
138			// We do not destroy the FactoryStorage singleton because the module will not
139			// be reloaded at next initialization (because Python C API cannot safely
140			// unload a module...).
141			// SignalCaster singleton could probably be destroyed.
142		15	dynamicgraph::PoolStorage::destroy();
143
144			// Load the symbols.
145	✓✓✓✓	15	if (sot_dynamic_library_ != nullptr && sot_external_interface_ != nullptr) {
146			destroySotExternalInterface_t *destroySotExternalInterface =
147			reinterpret_cast<destroySotExternalInterface_t *>(
148			reinterpret_cast<long>(
149		10	dlsym(sot_dynamic_library_, "destroySotExternalInterface")));
150		10	const char *dlsym_error = dlerror();
151	✗✓	10	if (dlsym_error) {
152			std::cerr << "Cannot load symbol destroy: " << dlsym_error << '\n';
153			return;
154			}
155
156		10	destroySotExternalInterface(sot_external_interface_);
157		10	sot_external_interface_ = nullptr;
158
159			/// Uncount the number of access to this library.
160		10	dlclose(sot_dynamic_library_);
161		10	sot_dynamic_library_ = nullptr;
162			}
163			}
164
165		118	void SotLoader::runPythonCommand(const std::string &command,
166			std::string &result, std::string &out,
167			std::string &err) {
168		118	embeded_python_interpreter_.python(command, result, out, err);
169		118	}
170
171		2	void SotLoader::oneIteration(
172			std::map<std::string, SensorValues> &sensors_in,
173			std::map<std::string, ControlValues> &control_values) {
174	✓✓	2	if (!dynamic_graph_stopped_) {
175			try {
176	✓✗	1	sot_external_interface_->nominalSetSensors(sensors_in);
177	✓✗	1	sot_external_interface_->getControl(control_values);
178			} catch (std::exception &e) {
179			std::cout << "Exception while running the graph:\n"
180			<< e.what() << std::endl;
181			throw e;
182			}
183			}
184		2	}
185
186		1	void SotLoader::loadDeviceInPython(const std::string &device_name) {
187		2	std::string result, out, err;
188			// Debug print.
189	✓✗✓✗	1	runPythonCommand("print(\"Load device from C++ to Python...\")", result, out,
190			err);
191
192			// Import the Device entity declaration
193	✓✗✓✗	1	runPythonCommand("from dynamic_graph.sot.core import Device", result, out,
194			err);
195
196			// Get the existing C++ entity pointer in the Python interpreter.
197	✓✗✓✗ ✓✗	1	runPythonCommand("loaded_device_name = \"" + device_name + "\"", result, out,
198			err);
199	✓✗✓✗	1	runPythonCommand("device_cpp_object = Device(loaded_device_name)", result,
200			out, err);
201
202			// Debug print.
203	✓✗✓✗	1	runPythonCommand("print(\"Load device from C++ to Python... Done!!\")",
204			result, out, err);
205
206			// strore the device name to unregister it upon cleanup.
207	✓✗	1	device_name_ = device_name;
208		1	}
209
210			} /* namespace sot */
211			} /* namespace dynamicgraph */