Directory: | ./ |
---|---|
File: | src/tools/sot-loader.cpp |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 81 | 92 | 88.0% |
Branches: | 94 | 196 | 48.0% |
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 |
1/2✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
|
13 | SotLoader::SotLoader() { |
34 | 13 | dynamic_graph_stopped_ = true; | |
35 | 13 | sot_external_interface_ = nullptr; | |
36 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | sot_dynamic_library_filename_ = ""; |
37 | 13 | sot_dynamic_library_ = nullptr; | |
38 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | device_name_ = ""; |
39 | 13 | } | |
40 | |||
41 | 13 | SotLoader::~SotLoader() { cleanUp(); } | |
42 | |||
43 | 2 | int SotLoader::parseOptions(int argc, char *argv[]) { | |
44 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
4 | po::options_description desc("Allowed options"); |
45 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | desc.add_options()("help", "produce help message")( |
46 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | "sot-dynamic-library", po::value<std::string>(), "Library to load"); |
47 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | desc.add_options()("help", "produce help message")( |
48 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | "input-file", po::value<std::string>(), "Library to load"); |
49 | |||
50 | // Input variable map from the command line (int argc, char* argv[]). | ||
51 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | po::variables_map vm; |
52 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
2 | po::store(po::parse_command_line(argc, argv, desc), vm); |
53 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | po::notify(vm); |
54 | |||
55 |
3/6✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 2 times.
|
2 | if (vm.count("help")) { |
56 | ✗ | std::cout << desc << "\n"; | |
57 | ✗ | return -1; | |
58 | } | ||
59 |
4/6✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 1 times.
|
2 | if (vm.count("sot-dynamic-library")) { |
60 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
|
1 | sot_dynamic_library_filename_ = vm["sot-dynamic-library"].as<std::string>(); |
61 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
1 | } else if (vm.count("input-file")) { |
62 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
|
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 | 2 | } | |
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 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
|
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 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 10 times.
|
12 | if (dlsym_error) { |
88 |
3/6✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
2 | std::cerr << "Cannot load symbol create: " << dlsym_error << '\n'; |
89 | 2 | return false; | |
90 | } | ||
91 | |||
92 | // Create robot-controller | ||
93 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | sot_external_interface_ = createSotExternalInterface(); |
94 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
|
10 | assert(sot_external_interface_ && "Fail to create the sotExternalInterface"); |
95 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | std::cout << "SoT loaded at address [" << &sot_external_interface_ |
96 |
4/8✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
|
10 | << "] from " << sot_dynamic_library_filename_ << "." << std::endl; |
97 | |||
98 | // Init the python interpreter. | ||
99 | 10 | std::string result, out, err; | |
100 | // Debug print. | ||
101 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
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 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
10 | runPythonCommand("import sys, os", result, out, err); |
106 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
10 | runPythonCommand("print(\"python version:\", sys.version)", result, out, err); |
107 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
10 | runPythonCommand("pythonpath = os.environ.get('PYTHONPATH', '')", result, out, |
108 | err); | ||
109 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
10 | runPythonCommand("path = []", result, out, err); |
110 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
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 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
10 | runPythonCommand("path.extend(sys.path)", result, out, err); |
116 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
10 | runPythonCommand("sys.path = path", result, out, err); |
117 | // used to be able to invoke rospy | ||
118 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
10 | runPythonCommand( |
119 | "if not hasattr(sys, \'argv\'):\n" | ||
120 | " sys.argv = ['sot']", | ||
121 | result, out, err); | ||
122 | // help setting signals | ||
123 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
10 | runPythonCommand("import numpy as np", result, out, err); |
124 | // Debug print. | ||
125 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
10 | runPythonCommand("print(\"Executing python interpreter prologue... Done\")", |
126 | result, out, err); | ||
127 | |||
128 | 10 | return true; | |
129 | 10 | } | |
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 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 14 times.
|
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 |
4/4✓ Branch 0 taken 12 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 10 times.
✓ Branch 3 taken 2 times.
|
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 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
|
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/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
|
2 | if (!dynamic_graph_stopped_) { |
175 | try { | ||
176 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | sot_external_interface_->nominalSetSensors(sensors_in); |
177 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
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 | 1 | std::string result, out, err; | |
188 | // Debug print. | ||
189 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | runPythonCommand("print(\"Load device from C++ to Python...\")", result, out, |
190 | err); | ||
191 | |||
192 | // Import the Device entity declaration | ||
193 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
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 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | runPythonCommand("loaded_device_name = \"" + device_name + "\"", result, out, |
198 | err); | ||
199 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | runPythonCommand("device_cpp_object = Device(loaded_device_name)", result, |
200 | out, err); | ||
201 | |||
202 | // Debug print. | ||
203 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
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/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | device_name_ = device_name; |
208 | 1 | } | |
209 | |||
210 | } /* namespace sot */ | ||
211 | } /* namespace dynamicgraph */ | ||
212 |