Line data Source code
1 : /*
2 : * Copyright (C) 2019 IIT-HHCM
3 : * Author: Arturo Laurenzi
4 : * email: arturo.laurenzi@iit.it
5 : *
6 : * Licensed under the Apache License, Version 2.0 (the "License");
7 : * you may not use this file except in compliance with the License.
8 : * You may obtain a copy of the License at
9 : * http://www.apache.org/licenses/LICENSE-2.0
10 : *
11 : * Unless required by applicable law or agreed to in writing, software
12 : * distributed under the License is distributed on an "AS IS" BASIS,
13 : * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 : * See the License for the specific language governing permissions and
15 : * limitations under the License.
16 : */
17 :
18 : #ifndef __ROSEE_UTILS__
19 : #define __ROSEE_UTILS__
20 :
21 : #include <cmath>
22 : #include <memory>
23 : #include <iostream>
24 : #include <dlfcn.h>
25 :
26 : //to find relative path for the config files and create directories
27 : #include <boost/filesystem.hpp>
28 : #include <fstream>
29 :
30 : #include <chrono>
31 : #include <atomic>
32 :
33 : namespace ROSEE
34 : {
35 :
36 : namespace Utils
37 : {
38 :
39 188 : static bool create_directory(std::string pathDirectory){
40 376 : boost::filesystem::path path(pathDirectory);
41 376 : return boost::filesystem::create_directories(path);
42 : }
43 :
44 188 : static void out2file ( std::string pathFile, std::string output) {
45 376 : std::ofstream fout ( pathFile );
46 188 : fout << output;
47 188 : }
48 :
49 : // static std::string get_environment_variable( std::string const & key )
50 : // {
51 : // char * val = getenv( key.c_str() );
52 : // return val == NULL ? std::string("") : std::string(val);
53 : // }
54 :
55 0 : static std::vector <std::string> getFilesInDir ( std::string pathFolder ) {
56 :
57 0 : boost::filesystem::path p (pathFolder);
58 0 : std::vector <std::string> retVect;
59 :
60 0 : if (! boost::filesystem::exists(p) ) {
61 0 : std::cerr << "[ERROR " << __func__ << "] path '" << pathFolder << "' does not exists" << std::endl;
62 0 : return retVect;
63 : }
64 :
65 0 : if (! boost::filesystem::is_directory(p)){
66 0 : std::cerr << "[ERROR " << __func__ << "] path '" << pathFolder << "' is not a directory" << std::endl;
67 0 : return retVect;
68 : }
69 :
70 0 : for (boost::filesystem::directory_entry& x : boost::filesystem::directory_iterator(p)) {
71 0 : retVect.push_back (x.path().filename().string() );
72 : }
73 :
74 0 : return retVect;
75 : }
76 :
77 10 : static inline int binomial_coefficent(int n, int k) {
78 :
79 10 : if (k == 0 || k == n){
80 6 : return 1;
81 : }
82 4 : return Utils::binomial_coefficent(n - 1, k - 1) + Utils::binomial_coefficent(n - 1, k);
83 :
84 : }
85 :
86 : template <class KeyType, class ValueType>
87 14 : static std::vector<KeyType> extract_keys(std::map<KeyType, ValueType> const& input_map) {
88 14 : std::vector<KeyType> retval;
89 45 : for (auto const& element : input_map) {
90 31 : retval.push_back(element.first);
91 : }
92 14 : return retval;
93 : }
94 :
95 : /**
96 : * @brief Extract all the string in the set keys of a map. All string are put togheter so
97 : * the original meaning of each set is lost
98 : * @param input_map the map where extract the keys
99 : * @param max_string_number the max number of different string among all the set keys.
100 : * Useful to not iterate all the map if not necessary. With default value = 0 all
101 : * map is iterated.
102 : * @return vector of extracted string of set keys (string in this vect will be unique)
103 : */
104 : template <class T>
105 : static std::vector<std::string> extract_keys_merged(
106 : std::map<std::set<std::string>, T> const& input_map, unsigned int max_string_number = 0) {
107 :
108 : std::set<std::string> allStrings;
109 : // if else so we do not check in the for the max_string_number if it is not used (ie ==0)
110 :
111 : if (max_string_number == 0) {
112 : for (auto const& element : input_map) {
113 : allStrings.insert( element.first.begin(), element.first.end() );
114 : }
115 :
116 : } else {
117 : for (auto const& element : input_map) {
118 : allStrings.insert(element.first.begin(), element.first.end());
119 : if (max_string_number == allStrings.size()){
120 : break;
121 : }
122 : if (max_string_number < allStrings.size() ) {
123 : std::cerr << "[ERROR]" << __func__ << " You passed " << max_string_number
124 : << " but I found more unique strings in the set keys ( " << allStrings.size()
125 : << " found)" << std::endl;
126 : return std::vector<std::string>();
127 : }
128 : }
129 : }
130 : std::vector<std::string> retval (allStrings.begin(), allStrings.end());
131 : return retval;
132 : }
133 :
134 : /**
135 : * @brief See above, this is the version with pair instead of set
136 : */
137 : template <class T>
138 : static std::vector<std::string> extract_keys_merged(
139 : std::map<std::pair<std::string,std::string>, T> const& input_map, unsigned int max_string_number = 0) {
140 :
141 : std::set<std::string> allStrings;
142 : // if else so we do not check in the for the max_string_number if it is not used (ie ==0)
143 :
144 : if (max_string_number == 0) {
145 : for (auto const& element : input_map) {
146 : allStrings.insert( element.first.first);
147 : allStrings.insert( element.first.second);
148 : }
149 :
150 : } else {
151 : for (auto const& element : input_map) {
152 : allStrings.insert( element.first.first);
153 : allStrings.insert( element.first.second);
154 : if (max_string_number == allStrings.size()){
155 : break;
156 : }
157 : if (max_string_number < allStrings.size() ) {
158 : std::cerr << "[ERROR]" << __func__ << " You passed " << max_string_number
159 : << " but I found more unique strings in the pair keys ( " << allStrings.size()
160 : << " found)" << std::endl;
161 : return std::vector<std::string>();
162 : }
163 : }
164 : }
165 : std::vector<std::string> retval (allStrings.begin(), allStrings.end());
166 : return retval;
167 : }
168 :
169 : /** @brief Return false if two maps have different keys.
170 : * The type of the keys (@p typename) must be the same obviously,
171 : * but the values (@p valueType1 and @p valueType2) can be anything, because they are not considered
172 : */
173 : template <typename keyType, typename valueType1, typename valueType2>
174 79 : bool keys_equal (std::map <keyType, valueType1> const &lhs, std::map<keyType, valueType2> const &rhs) {
175 :
176 512 : auto pred = [] (decltype(*lhs.begin()) a, decltype(*rhs.begin()) b)
177 512 : { return (a.first == b.first); };
178 :
179 :
180 79 : return lhs.size() == rhs.size()
181 79 : && std::equal(lhs.begin(), lhs.end(), rhs.begin(), pred);
182 : }
183 :
184 : template <typename Map1, typename Map2>
185 : struct DifferentKeysException : public std::exception {
186 : const Map1 *map1;
187 : const Map2 *map2;
188 :
189 0 : DifferentKeysException(const Map1 *map1, const Map2 *map2) :
190 0 : map1(map1), map2(map2) {}
191 :
192 0 : const char * what () const throw () {
193 0 : std::stringstream output;
194 0 : output << "First map keys:\n";
195 0 : for (auto it : *map1) {
196 0 : output << "\t" << it.first << "\n";
197 : }
198 0 : output << ("Second map keys:\n");
199 0 : for (auto it : *map2) {
200 0 : output << "\t" << it.first << "\n";
201 : }
202 0 : std::cerr << output.str().c_str() << std::endl;
203 :
204 0 : return "Maps have different keys";
205 : }
206 : };
207 :
208 : /**
209 : * @brief Utils to dynamically load an object. This is used to dynamically load
210 : * a derived object from a node that only knows the base interface.
211 : * For example, we call the create_object(rclcpp::Node node) method of a derived EEHAL class
212 : * The object must be a library which will return a RetType pointer with the \p function_name
213 : * This function will "convert" to smart pointer for convenience
214 : * @param lib_name the name of the compiled library (eg DummyHal). Do not add the suffix .so
215 : * @param function_name The method of \param lib_name which will return a RetType*.
216 : * @param args arguments for the \p function_name , if the case
217 : * @return std::shared_ptr<RetType> a pointer to the new created object
218 : */
219 : template <typename RetType, typename... Args>
220 : std::unique_ptr<RetType> loadObject(std::string lib_name,
221 : std::string function_name,
222 : Args... args) {
223 :
224 : if (lib_name.empty()) {
225 :
226 : std::cerr << "[Utils::loadObject] ERROR: Please specify lib_name" << std::endl;
227 : return nullptr;
228 : }
229 :
230 : std::string lib_name_path = "lib" + lib_name +".so";
231 :
232 : //clear old errors
233 : dlerror();
234 : std::cout << "aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa" << std::endl;
235 :
236 : void* lib_handle = dlopen(lib_name_path.c_str(), RTLD_LAZY);
237 : auto error = dlerror();
238 :
239 : if (!lib_handle || error != NULL) {
240 : std::cerr << "[Utils::loadObject] ERROR in opening the library: " << error << std::endl;
241 : return nullptr;
242 : }
243 :
244 : //clear old errors
245 : dlerror();
246 :
247 : std::cout << "bbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbb" << std::endl;
248 :
249 :
250 : RetType* (*function)(Args... args);
251 : function = reinterpret_cast<RetType* (*)(Args... args)>(dlsym(lib_handle, function_name.c_str()));
252 : error = dlerror();
253 : if ( error != NULL) {
254 : std::cerr << "[Utils::loadObject] ERROR in returning the function: " << error << std::endl;
255 : return nullptr;
256 : }
257 : std::cout << "ccccccccccccccccccccccccccccccccccccccccccccccccc" << std::endl;
258 :
259 : RetType* objectRaw = function(args...);
260 : std::cout << "ddddddddddddddddddddddddddddd" << std::endl;
261 :
262 : std::unique_ptr<RetType> objectPtr(objectRaw);
263 : std::cout << "eeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeee" << std::endl;
264 :
265 : dlclose(lib_handle);
266 : std::cout << "fffffffffffffffffffffffffffffff" << std::endl;
267 :
268 : return objectPtr;
269 : }
270 :
271 : //default template as high_resolution_clock
272 : //copied from https://codereview.stackexchange.com/questions/196245/extremely-simple-timer-class-in-c
273 : template <typename Clock = std::chrono::high_resolution_clock>
274 : class Timer
275 : {
276 : typename Clock::time_point start_point;
277 :
278 : public:
279 : Timer() : start_point(Clock::now()) {}
280 :
281 : void reset() { start_point = Clock::now(); }
282 :
283 : template <typename Rep = typename Clock::duration::rep, typename Units = typename Clock::duration>
284 : Rep elapsed_time() const
285 : {
286 : std::atomic_thread_fence(std::memory_order_relaxed);
287 : auto counted_time = std::chrono::duration_cast<Units>(Clock::now() - start_point).count();
288 : std::atomic_thread_fence(std::memory_order_relaxed);
289 : return static_cast<Rep>(counted_time);
290 : }
291 : };
292 :
293 : template <typename SignalType>
294 : class SecondOrderFilter
295 : {
296 :
297 : public:
298 :
299 : typedef std::shared_ptr<SecondOrderFilter<SignalType>> Ptr;
300 :
301 : SecondOrderFilter() :
302 : _omega ( 1.0 ),
303 : _eps ( 0.8 ),
304 : _ts ( 0.01 ),
305 : _reset_has_been_called ( false )
306 : {
307 : computeCoeff();
308 : }
309 :
310 : SecondOrderFilter ( double omega, double eps, double ts, const SignalType& initial_state ) :
311 : _omega ( omega ),
312 : _eps ( eps ),
313 : _ts ( ts ),
314 : _reset_has_been_called ( false )
315 : {
316 : computeCoeff();
317 : reset ( initial_state );
318 : }
319 :
320 : void reset ( const SignalType& initial_state )
321 : {
322 : _reset_has_been_called = true;
323 : _u = initial_state;
324 : _y = initial_state;
325 : _yd = initial_state;
326 : _ydd = initial_state;
327 : _udd = initial_state;
328 : _ud = initial_state;
329 : }
330 :
331 : const SignalType& process ( const SignalType& input )
332 : {
333 :
334 : if ( !_reset_has_been_called ) {
335 : reset ( input*0 );
336 : }
337 :
338 :
339 : _ydd = _yd;
340 : _yd = _y;
341 : _udd = _ud;
342 : _ud = _u;
343 :
344 :
345 : _u = input;
346 : _y = 1.0/_a0 * ( _u + _b1*_ud + _b2*_udd - _a1*_yd - _a2*_ydd );
347 :
348 : return _y;
349 : }
350 :
351 : const SignalType& getOutput() const
352 : {
353 : return _y;
354 : }
355 :
356 : void setOmega ( double omega )
357 : {
358 : _omega = omega;
359 : computeCoeff();
360 : }
361 :
362 : double getOmega()
363 : {
364 : return _omega;
365 : }
366 :
367 : void setDamping ( double eps )
368 : {
369 : _eps = eps;
370 : computeCoeff();
371 : }
372 :
373 : double getDamping()
374 : {
375 : return _eps;
376 : }
377 :
378 : void setTimeStep ( double ts )
379 : {
380 : _ts = ts;
381 : computeCoeff();
382 : }
383 :
384 : double getTimeStep()
385 : {
386 : return _ts;
387 : }
388 :
389 : private:
390 :
391 : void computeCoeff()
392 : {
393 : _b1 = 2.0;
394 : _b2 = 1.0;
395 :
396 : _a0 = 1.0 + 4.0*_eps/ ( _omega*_ts ) + 4.0/std::pow ( _omega*_ts, 2.0 );
397 : _a1 = 2 - 8.0/std::pow ( _omega*_ts, 2.0 );
398 : _a2 = 1.0 + 4.0/std::pow ( _omega*_ts, 2.0 ) - 4.0*_eps/ ( _omega*_ts );
399 :
400 : }
401 :
402 : double _omega;
403 : double _eps;
404 : double _ts;
405 :
406 : double _b1, _b2;
407 : double _a0, _a1, _a2;
408 :
409 : bool _reset_has_been_called;
410 :
411 : SignalType _y, _yd, _ydd, _u, _ud, _udd;
412 :
413 : };
414 :
415 :
416 : }
417 :
418 : }
419 :
420 : #endif // __ROSEE_UTILS__
|
