GCC Code Coverage Report

Directory: ./
File: src/tools/sot-loader.cpp
Date: 2025-05-13 12:28:21
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