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 192 : static bool create_directory(std::string pathDirectory){
40 384 : boost::filesystem::path path(pathDirectory);
41 384 : return boost::filesystem::create_directories(path);
42 : }
43 :
44 192 : static void out2file ( std::string pathFile, std::string output) {
45 384 : std::ofstream fout ( pathFile );
46 192 : fout << output;
47 192 : }
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 123 : static std::vector <std::string> getFilesInDir ( std::string pathFolder ) {
56 :
57 246 : boost::filesystem::path p (pathFolder);
58 123 : std::vector <std::string> retVect;
59 :
60 123 : 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 123 : 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 638 : for (boost::filesystem::directory_entry& x : boost::filesystem::directory_iterator(p)) {
71 515 : retVect.push_back (x.path().filename().string() );
72 : }
73 :
74 123 : 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 :
87 551 : static std::string getPackagePath() {
88 :
89 1102 : boost::filesystem::path path(__FILE__);
90 551 : path.remove_filename();
91 1102 : return path.string() + "/../../";
92 : }
93 :
94 : template <class KeyType, class ValueType>
95 14 : static std::vector<KeyType> extract_keys(std::map<KeyType, ValueType> const& input_map) {
96 14 : std::vector<KeyType> retval;
97 45 : for (auto const& element : input_map) {
98 31 : retval.push_back(element.first);
99 : }
100 14 : return retval;
101 : }
102 :
103 : /**
104 : * @brief Extract all the string in the set keys of a map. All string are put togheter so
105 : * the original meaning of each set is lost
106 : * @param input_map the map where extract the keys
107 : * @param max_string_number the max number of different string among all the set keys.
108 : * Useful to not iterate all the map if not necessary. With default value = 0 all
109 : * map is iterated.
110 : * @return vector of extracted string of set keys (string in this vect will be unique)
111 : */
112 : template <class T>
113 0 : static std::vector<std::string> extract_keys_merged(
114 : std::map<std::set<std::string>, T> const& input_map, unsigned int max_string_number = 0) {
115 :
116 0 : std::set<std::string> allStrings;
117 : // if else so we do not check in the for the max_string_number if it is not used (ie ==0)
118 :
119 0 : if (max_string_number == 0) {
120 0 : for (auto const& element : input_map) {
121 0 : allStrings.insert( element.first.begin(), element.first.end() );
122 : }
123 :
124 : } else {
125 0 : for (auto const& element : input_map) {
126 0 : allStrings.insert(element.first.begin(), element.first.end());
127 0 : if (max_string_number == allStrings.size()){
128 0 : break;
129 : }
130 0 : if (max_string_number < allStrings.size() ) {
131 0 : std::cerr << "[ERROR]" << __func__ << " You passed " << max_string_number
132 0 : << " but I found more unique strings in the set keys ( " << allStrings.size()
133 0 : << " found)" << std::endl;
134 0 : return std::vector<std::string>();
135 : }
136 : }
137 : }
138 0 : std::vector<std::string> retval (allStrings.begin(), allStrings.end());
139 0 : return retval;
140 : }
141 :
142 : /**
143 : * @brief See above, this is the version with pair instead of set
144 : */
145 : template <class T>
146 : static std::vector<std::string> extract_keys_merged(
147 : std::map<std::pair<std::string,std::string>, T> const& input_map, unsigned int max_string_number = 0) {
148 :
149 : std::set<std::string> allStrings;
150 : // if else so we do not check in the for the max_string_number if it is not used (ie ==0)
151 :
152 : if (max_string_number == 0) {
153 : for (auto const& element : input_map) {
154 : allStrings.insert( element.first.first);
155 : allStrings.insert( element.first.second);
156 : }
157 :
158 : } else {
159 : for (auto const& element : input_map) {
160 : allStrings.insert( element.first.first);
161 : allStrings.insert( element.first.second);
162 : if (max_string_number == allStrings.size()){
163 : break;
164 : }
165 : if (max_string_number < allStrings.size() ) {
166 : std::cerr << "[ERROR]" << __func__ << " You passed " << max_string_number
167 : << " but I found more unique strings in the pair keys ( " << allStrings.size()
168 : << " found)" << std::endl;
169 : return std::vector<std::string>();
170 : }
171 : }
172 : }
173 : std::vector<std::string> retval (allStrings.begin(), allStrings.end());
174 : return retval;
175 : }
176 :
177 : /** @brief Return false if two maps have different keys.
178 : * The type of the keys (@p typename) must be the same obviously,
179 : * but the values (@p valueType1 and @p valueType2) can be anything, because they are not considered
180 : */
181 : template <typename keyType, typename valueType1, typename valueType2>
182 296 : bool keys_equal (std::map <keyType, valueType1> const &lhs, std::map<keyType, valueType2> const &rhs) {
183 :
184 1266 : auto pred = [] (decltype(*lhs.begin()) a, decltype(*rhs.begin()) b)
185 1266 : { return (a.first == b.first); };
186 :
187 :
188 296 : return lhs.size() == rhs.size()
189 296 : && std::equal(lhs.begin(), lhs.end(), rhs.begin(), pred);
190 : }
191 :
192 : template <typename Map1, typename Map2>
193 12 : struct DifferentKeysException : public std::exception {
194 : const Map1 *map1;
195 : const Map2 *map2;
196 :
197 4 : DifferentKeysException(const Map1 *map1, const Map2 *map2) :
198 4 : map1(map1), map2(map2) {}
199 :
200 0 : const char * what () const throw () {
201 0 : std::stringstream output;
202 0 : output << "First map keys:\n";
203 0 : for (auto it : *map1) {
204 0 : output << "\t" << it.first << "\n";
205 : }
206 0 : output << ("Second map keys:\n");
207 0 : for (auto it : *map2) {
208 0 : output << "\t" << it.first << "\n";
209 : }
210 0 : std::cerr << output.str().c_str() << std::endl;
211 :
212 0 : return "Maps have different keys";
213 : }
214 : };
215 :
216 : /**
217 : * @brief Utils to dynamically load an object. This is used to dynamically load
218 : * a derived object from a node that only knows the base interface.
219 : * For example, we call the create_object(ros::nodeHandle) method of a derived EEHAL class
220 : * The object must be a library which will return a RetType pointer with the \p function_name
221 : * This function will "convert" to smart pointer for convenience
222 : * @param lib_name the name of the compiled library (eg DummyHal). Do not add the suffix .so
223 : * @param function_name The method of \param lib_name which will return a RetType*.
224 : * @param args arguments for the \p function_name , if the case
225 : * @return std::shared_ptr<RetType> a pointer to the new created object
226 : */
227 : template <typename RetType, typename... Args>
228 41 : std::unique_ptr<RetType> loadObject(std::string lib_name,
229 : std::string function_name,
230 : Args... args) {
231 :
232 41 : if (lib_name.empty()) {
233 :
234 0 : std::cerr << "[Utils::loadObject] ERROR: Please specify lib_name" << std::endl;
235 0 : return nullptr;
236 : }
237 :
238 82 : std::string lib_name_path = "lib" + lib_name +".so";
239 :
240 : //clear old errors
241 41 : dlerror();
242 :
243 41 : void* lib_handle = dlopen(lib_name_path.c_str(), RTLD_LAZY);
244 41 : auto error = dlerror();
245 :
246 41 : if (!lib_handle || error != NULL) {
247 0 : std::cerr << "[Utils::loadObject] ERROR in opening the library: " << error << std::endl;
248 0 : return nullptr;
249 : }
250 :
251 : //clear old errors
252 41 : dlerror();
253 :
254 : RetType* (*function)(Args... args);
255 41 : function = reinterpret_cast<RetType* (*)(Args... args)>(dlsym(lib_handle, function_name.c_str()));
256 41 : error = dlerror();
257 41 : if ( error != NULL) {
258 0 : std::cerr << "[Utils::loadObject] ERROR in returning the function: " << error << std::endl;
259 0 : return nullptr;
260 : }
261 :
262 41 : RetType* objectRaw = function(args...);
263 :
264 82 : std::unique_ptr<RetType> objectPtr(objectRaw);
265 :
266 41 : dlclose(lib_handle);
267 :
268 41 : 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 41 : Timer() : start_point(Clock::now()) {}
280 :
281 9 : void reset() { start_point = Clock::now(); }
282 :
283 : template <typename Rep = typename Clock::duration::rep, typename Units = typename Clock::duration>
284 942 : Rep elapsed_time() const
285 : {
286 : std::atomic_thread_fence(std::memory_order_relaxed);
287 942 : auto counted_time = std::chrono::duration_cast<Units>(Clock::now() - start_point).count();
288 : std::atomic_thread_fence(std::memory_order_relaxed);
289 942 : return static_cast<Rep>(counted_time);
290 : }
291 : };
292 :
293 : template <typename SignalType>
294 41 : class SecondOrderFilter
295 : {
296 :
297 : public:
298 :
299 : typedef std::shared_ptr<SecondOrderFilter<SignalType>> Ptr;
300 :
301 41 : SecondOrderFilter() :
302 : _omega ( 1.0 ),
303 : _eps ( 0.8 ),
304 : _ts ( 0.01 ),
305 41 : _reset_has_been_called ( false )
306 : {
307 41 : computeCoeff();
308 41 : }
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 41 : void reset ( const SignalType& initial_state )
321 : {
322 41 : _reset_has_been_called = true;
323 41 : _u = initial_state;
324 41 : _y = initial_state;
325 41 : _yd = initial_state;
326 41 : _ydd = initial_state;
327 41 : _udd = initial_state;
328 41 : _ud = initial_state;
329 41 : }
330 :
331 21341 : const SignalType& process ( const SignalType& input )
332 : {
333 :
334 21341 : if ( !_reset_has_been_called ) {
335 0 : reset ( input*0 );
336 : }
337 :
338 :
339 21341 : _ydd = _yd;
340 21341 : _yd = _y;
341 21341 : _udd = _ud;
342 21341 : _ud = _u;
343 :
344 :
345 21341 : _u = input;
346 21341 : _y = 1.0/_a0 * ( _u + _b1*_ud + _b2*_udd - _a1*_yd - _a2*_ydd );
347 :
348 21341 : return _y;
349 : }
350 :
351 : const SignalType& getOutput() const
352 : {
353 : return _y;
354 : }
355 :
356 41 : void setOmega ( double omega )
357 : {
358 41 : _omega = omega;
359 41 : computeCoeff();
360 41 : }
361 :
362 : double getOmega()
363 : {
364 : return _omega;
365 : }
366 :
367 41 : void setDamping ( double eps )
368 : {
369 41 : _eps = eps;
370 41 : computeCoeff();
371 41 : }
372 :
373 : double getDamping()
374 : {
375 : return _eps;
376 : }
377 :
378 41 : void setTimeStep ( double ts )
379 : {
380 41 : _ts = ts;
381 41 : computeCoeff();
382 41 : }
383 :
384 : double getTimeStep()
385 : {
386 : return _ts;
387 : }
388 :
389 : private:
390 :
391 164 : void computeCoeff()
392 : {
393 164 : _b1 = 2.0;
394 164 : _b2 = 1.0;
395 :
396 164 : _a0 = 1.0 + 4.0*_eps/ ( _omega*_ts ) + 4.0/std::pow ( _omega*_ts, 2.0 );
397 164 : _a1 = 2 - 8.0/std::pow ( _omega*_ts, 2.0 );
398 164 : _a2 = 1.0 + 4.0/std::pow ( _omega*_ts, 2.0 ) - 4.0*_eps/ ( _omega*_ts );
399 :
400 164 : }
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__
|
