sot-loader.cpp
Go to the documentation of this file.
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 
35  sot_external_interface_ = nullptr;
37  sot_dynamic_library_ = nullptr;
38  device_name_ = "";
39 }
40 
42 
43 int SotLoader::parseOptions(int argc, char *argv[]) {
44  po::options_description desc("Allowed options");
45  desc.add_options()("help", "produce help message")(
46  "sot-dynamic-library", po::value<std::string>(), "Library to load");
47  desc.add_options()("help", "produce help message")(
48  "input-file", po::value<std::string>(), "Library to load");
49 
50  // Input variable map from the command line (int argc, char* argv[]).
51  po::variables_map vm;
52  po::store(po::parse_command_line(argc, argv, desc), vm);
53  po::notify(vm);
54 
55  if (vm.count("help")) {
56  std::cout << desc << "\n";
57  return -1;
58  }
59  if (vm.count("sot-dynamic-library")) {
60  sot_dynamic_library_filename_ = vm["sot-dynamic-library"].as<std::string>();
61  } else if (vm.count("input-file")) {
62  sot_dynamic_library_filename_ = vm["input-file"].as<std::string>();
63  } else {
64  std::cout << "No filename specified\n";
65  return -1;
66  }
67  return 0;
68 }
69 
71  // Load the library containing the AbstractSotExternalInterface.
73  dlopen(sot_dynamic_library_filename_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
74  if (!sot_dynamic_library_) {
75  std::cerr << "Cannot load library: " << dlerror() << '\n';
76  return false;
77  }
78 
79  // reset errors
80  dlerror();
81 
82  // Load the symbols.
84  reinterpret_cast<createSotExternalInterface_t *>(reinterpret_cast<long>(
85  dlsym(sot_dynamic_library_, "createSotExternalInterface")));
86  const char *dlsym_error = dlerror();
87  if (dlsym_error) {
88  std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
89  return false;
90  }
91 
92  // Create robot-controller
94  assert(sot_external_interface_ && "Fail to create the sotExternalInterface");
95  std::cout << "SoT loaded at address [" << &sot_external_interface_
96  << "] from " << sot_dynamic_library_filename_ << "." << std::endl;
97 
98  // Init the python interpreter.
99  std::string result, out, err;
100  // Debug print.
101  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  runPythonCommand("import sys, os", result, out, err);
106  runPythonCommand("print(\"python version:\", sys.version)", result, out, err);
107  runPythonCommand("pythonpath = os.environ.get('PYTHONPATH', '')", result, out,
108  err);
109  runPythonCommand("path = []", result, out, err);
111  "for p in pythonpath.split(':'):\n"
112  " if p not in sys.path:\n"
113  " path.append(p)",
114  result, out, err);
115  runPythonCommand("path.extend(sys.path)", result, out, err);
116  runPythonCommand("sys.path = path", result, out, err);
117  // used to be able to invoke rospy
119  "if not hasattr(sys, \'argv\'):\n"
120  " sys.argv = ['sot']",
121  result, out, err);
122  // help setting signals
123  runPythonCommand("import numpy as np", result, out, err);
124  // Debug print.
125  runPythonCommand("print(\"Executing python interpreter prologue... Done\")",
126  result, out, err);
127 
128  return true;
129 }
130 
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  if (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.
143 
144  // Load the symbols.
145  if (sot_dynamic_library_ != nullptr && sot_external_interface_ != nullptr) {
147  reinterpret_cast<destroySotExternalInterface_t *>(
148  reinterpret_cast<long>(
149  dlsym(sot_dynamic_library_, "destroySotExternalInterface")));
150  const char *dlsym_error = dlerror();
151  if (dlsym_error) {
152  std::cerr << "Cannot load symbol destroy: " << dlsym_error << '\n';
153  return;
154  }
155 
157  sot_external_interface_ = nullptr;
158 
160  dlclose(sot_dynamic_library_);
161  sot_dynamic_library_ = nullptr;
162  }
163 }
164 
165 void SotLoader::runPythonCommand(const std::string &command,
166  std::string &result, std::string &out,
167  std::string &err) {
168  embeded_python_interpreter_.python(command, result, out, err);
169 }
170 
172  std::map<std::string, SensorValues> &sensors_in,
173  std::map<std::string, ControlValues> &control_values,
174  const double &period) {
175  if (!dynamic_graph_stopped_) {
176  try {
178  sot_external_interface_->getControl(control_values, period);
179  } catch (std::exception &e) {
180  std::cout << "Exception while running the graph:\n"
181  << e.what() << std::endl;
182  throw e;
183  }
184  }
185 }
186 
187 void SotLoader::loadDeviceInPython(const std::string &device_name) {
188  std::string result, out, err;
189  // Debug print.
190  runPythonCommand("print(\"Load device from C++ to Python...\")", result, out,
191  err);
192 
193  // Import the Device entity declaration
194  runPythonCommand("from dynamic_graph.sot.core import Device", result, out,
195  err);
196 
197  // Get the existing C++ entity pointer in the Python interpreter.
198  runPythonCommand("loaded_device_name = \"" + device_name + "\"", result, out,
199  err);
200  runPythonCommand("device_cpp_object = Device(loaded_device_name)", result,
201  out, err);
202 
203  // Debug print.
204  runPythonCommand("print(\"Load device from C++ to Python... Done!!\")",
205  result, out, err);
206 
207  // strore the device name to unregister it upon cleanup.
208  device_name_ = device_name;
209 }
210 
211 } /* namespace sot */
212 } /* namespace dynamicgraph */
void deregisterEntity(const std::string &entname)
void * sot_dynamic_library_
Handle on the SoT library.
Definition: sot-loader.hh:60
void loadDeviceInPython(const std::string &device_name)
Load the Device entity in the python global scope.
Definition: sot-loader.cpp:187
void destroySotExternalInterface(dynamicgraph::sot::AbstractSotExternalInterface *p)
bool initialization()
Prepare the SoT framework.
Definition: sot-loader.cpp:70
virtual void nominalSetSensors(std::map< std::string, SensorValues > &sensorsIn)=0
~SotLoader()
Default destructor.
Definition: sot-loader.cpp:41
void oneIteration(std::map< std::string, SensorValues > &sensors_in, std::map< std::string, ControlValues > &control_values, const double &period)
Compute one iteration of control. Basically executes fillSensors, the SoT and the readControl...
Definition: sot-loader.cpp:171
std::string device_name_
Device entity created and loaded, so we deregister it as the Pool is not responsible for it&#39;s life ti...
Definition: sot-loader.hh:73
std::string python(const std::string &command)
void cleanUp()
Unload the library which handles the robot device.
Definition: sot-loader.cpp:131
static PoolStorage * getInstance()
Definition: pool.cpp:147
static void destroy()
Definition: pool.cpp:153
SotLoader()
Default constructor.
Definition: sot-loader.cpp:33
python::Interpreter embeded_python_interpreter_
Embeded python interpreter.
Definition: sot-loader.hh:63
AbstractSotExternalInterface * sot_external_interface_
The interface between the device and the robot driver.
Definition: sot-loader.hh:53
dynamicgraph::sot::AbstractSotExternalInterface * createSotExternalInterface_t()
void runPythonCommand(const std::string &command, std::string &result, std::string &out, std::string &err)
Run a python command inside the embeded python interpreter.
Definition: sot-loader.cpp:165
dynamicgraph::sot::AbstractSotExternalInterface * createSotExternalInterface()
std::string sot_dynamic_library_filename_
Name of the dynamic library containing the dgs::AbstractSotExternalInterface object.
Definition: sot-loader.hh:57
void destroySotExternalInterface_t(dynamicgraph::sot::AbstractSotExternalInterface *)
err
virtual void getControl(std::map< std::string, ControlValues > &, const double &period=0)=0
int parseOptions(int argc, char *argv[])
Read user input to extract the path of the SoT dynamic library.
Definition: sot-loader.cpp:43
bool dynamic_graph_stopped_
Check if the dynamic graph is running or not.
Definition: sot-loader.hh:50


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26