Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpQbDevice.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Interface for the qb robotics devices.
33 *
34*****************************************************************************/
35
36#include <visp3/core/vpConfig.h>
37#ifdef VISP_HAVE_QBDEVICE
38
39#include <regex>
40
41#include <qb_device_driver.h>
42
43#include <visp3/core/vpIoTools.h>
44#include <visp3/robot/vpQbDevice.h>
45
46#ifndef DOXYGEN_SHOULD_SKIP_THIS
47class vpQbDevice::Impl
48{
49public:
50 Impl() : Impl(std::make_shared<qb_device_driver::qbDeviceAPI>()) {}
51 Impl(std::shared_ptr<qb_device_driver::qbDeviceAPI> device_api)
52 : m_serial_protectors(), m_connected_devices(), m_position_limits(2), m_device_api(device_api),
53 m_file_descriptors(), m_max_repeats(1), m_current_max(750.)
54 {
55 // Default values updated after a call to init()
56 m_position_limits[0] = 0;
57 m_position_limits[1] = 19000;
58 }
59
60 virtual ~Impl()
61 {
62 for (auto it = m_file_descriptors.begin(); it != m_file_descriptors.end();) {
63 if (close(it->first)) {
64 it = m_file_descriptors.erase(it);
65 } else {
66 ++it;
67 }
68 }
69 }
70
71 virtual int activate(const int &id, const bool &command, const int &max_repeats);
72 virtual int activate(const int &id, const int &max_repeats) { return activate(id, true, max_repeats); }
73
74 virtual bool close(const std::string &serial_port);
75
76 virtual int deactivate(const int &id, const int &max_repeats) { return activate(id, false, max_repeats); }
77
78 inline double getCurrentMax() const { return m_current_max; }
79
80 virtual int getCurrents(const int &id, const int &max_repeats, std::vector<short int> &currents);
81 virtual int getInfo(const int &id, const int &max_repeats, std::string &info);
82 virtual int getMeasurements(const int &id, const int &max_repeats, std::vector<short int> &currents,
83 std::vector<short int> &positions);
84 virtual int getParameters(const int &id, std::vector<int> &limits, std::vector<int> &resolutions);
85
86 std::vector<short int> getPositionLimits() const { return m_position_limits; }
87
88 virtual int getPositions(const int &id, const int &max_repeats, std::vector<short int> &positions);
89 virtual int getSerialPortsAndDevices(const int &max_repeats);
90 virtual bool init(const int &id);
91 virtual int isActive(const int &id, const int &max_repeats, bool &status);
92
93 virtual int isConnected(const int &id, const int &max_repeats);
94
95 virtual bool isInConnectedSet(const int &id) { return (m_connected_devices.count(id) ? true : false); }
96
97 virtual bool isInOpenMap(const std::string &serial_port)
98 {
99 return (m_file_descriptors.count(serial_port) ? true : false);
100 }
101
102 inline bool isReliable(int const &failures, int const &max_repeats)
103 {
104 return failures >= 0 && failures <= max_repeats;
105 }
106 virtual int open(const std::string &serial_port);
107
108 virtual int setCommandsAndWait(const int &id, const int &max_repeats, std::vector<short int> &commands);
109 virtual int setCommandsAsync(const int &id, std::vector<short int> &commands);
110
111 void setMaxRepeats(const int &max_repeats) { m_max_repeats = max_repeats; }
112
113public:
114 std::map<std::string, std::unique_ptr<std::mutex> >
115 m_serial_protectors; // only callbacks must lock the serial resources
116 std::map<int, std::string> m_connected_devices;
117
118protected:
119#if (defined(_WIN32) || defined(_WIN64))
120 std::unique_ptr<std::mutex> m_mutex_dummy; // FS: cannot build without this line with msvc
121#endif
122 std::vector<short int> m_position_limits; // min and max position values in ticks
123 std::shared_ptr<qb_device_driver::qbDeviceAPI> m_device_api;
124 std::map<std::string, comm_settings> m_file_descriptors;
125 int m_max_repeats;
126 double m_current_max;
127};
128
129int vpQbDevice::Impl::activate(const int &id, const bool &command, const int &max_repeats)
130{
131 std::string command_prefix = command ? "" : "de";
132 bool status = false;
133 int failures = 0;
134
135 failures = isActive(id, max_repeats, status);
136 if (status != command) {
137 m_device_api->activate(&m_file_descriptors.at(m_connected_devices.at(id)), id, command);
138 failures = std::max(failures, isActive(id, max_repeats, status));
139 if (status != command) {
140 std::cout << "Device [" << id << "] fails on " << command_prefix << "activation." << std::endl;
141 ;
142 return -1;
143 }
144 std::cout << "Device [" << id << "] motors have been " << command_prefix << "activated!" << std::endl;
145 return failures;
146 }
147 std::cout << "Device [" << id << "] motors were already " << command_prefix << "activated!" << std::endl;
148 return failures;
149}
150
151bool vpQbDevice::Impl::close(const std::string &serial_port)
152{
153 if (!isInOpenMap(serial_port)) {
154 std::cout << "has not handled [" << serial_port << "]." << std::endl;
155 return false; // no error: the communication is close anyway
156 }
157
158 for (auto const &device : m_connected_devices) {
159 if (device.second == serial_port) {
160 deactivate(device.first, m_max_repeats);
161 m_connected_devices.erase(device.first);
162 break;
163 }
164 }
165
166 m_device_api->close(&m_file_descriptors.at(serial_port));
167
168 // Note that m_file_descriptors.erase(serial_port) is done in the destructor.
169 // Cannot be done here since the iterator that is used in the destructor would be lost
170
171 std::cout << "does not handle [" << serial_port << "] anymore." << std::endl;
172 return true;
173}
174
175int vpQbDevice::Impl::getCurrents(const int &id, const int &max_repeats, std::vector<short int> &currents)
176{
177 // the API methods are called at most (i.e. very unlikely) 'max_repeats' times to guarantee the correct identification
178 // of a real fault in the communication
179 int failures = 0;
180 currents.resize(2); // required by 'getCurrents()'
181 std::lock_guard<std::mutex> serial_lock(*m_serial_protectors.at(m_connected_devices.at(id)));
182 while (failures <= max_repeats) {
183 if (m_device_api->getCurrents(&m_file_descriptors.at(m_connected_devices.at(id)), id, currents) < 0) {
184 failures++;
185 continue;
186 }
187 break;
188 }
189 return failures;
190}
191
192int vpQbDevice::Impl::getInfo(const int &id, const int &max_repeats, std::string &info)
193{
194 // the API methods are called at most (i.e. very unlikely) 'max_repeats' times to guarantee the correct identification
195 // of a real fault in the communication
196 int failures = 0;
197 while (failures <= max_repeats) {
198 info = m_device_api->getInfo(&m_file_descriptors.at(m_connected_devices.at(id)), id);
199 if (info == "") {
200 failures++;
201 continue;
202 }
203 break;
204 }
205 return failures;
206}
207
208int vpQbDevice::Impl::getMeasurements(const int &id, const int &max_repeats, std::vector<short int> &currents,
209 std::vector<short int> &positions)
210{
211 // the API methods are called at most (i.e. very unlikely) 'max_repeats' times to guarantee the correct identification
212 // of a real fault in the communication
213 int failures = 0;
214 currents.resize(2);
215 positions.resize(3);
216 std::vector<short int> measurements(5, 0); // required by 'getMeasurements()'
217 while (failures <= max_repeats) {
218 if (m_device_api->getMeasurements(&m_file_descriptors.at(m_connected_devices.at(id)), id, measurements) < 0) {
219 failures++;
220 continue;
221 }
222 std::copy(measurements.begin(), measurements.begin() + 2, currents.begin());
223 std::copy(measurements.begin() + 2, measurements.end(), positions.begin());
224 break;
225 }
226 return failures;
227}
228
229int vpQbDevice::Impl::getParameters(const int &id, std::vector<int> &limits, std::vector<int> &resolutions)
230{
231 std::vector<int> input_mode = {-1};
232 std::vector<int> control_mode = {-1};
233 m_device_api->getParameters(&m_file_descriptors.at(m_connected_devices.at(id)), id, input_mode, control_mode,
234 resolutions, limits);
235 if (!input_mode.front() && !control_mode.front()) { // both input and control modes equals 0 are required, i.e.
236 // respectively USB connected and position controlled
237 return 0;
238 }
239 return -1;
240}
241
242int vpQbDevice::Impl::getPositions(const int &id, const int &max_repeats, std::vector<short int> &positions)
243{
244 // the API methods are called at most (i.e. very unlikely) 'max_repeats' times to guarantee the correct identification
245 // of a real fault in the communication
246 int failures = 0;
247 positions.resize(3); // required by 'getPositions()'
248 std::lock_guard<std::mutex> serial_lock(*m_serial_protectors.at(m_connected_devices.at(id)));
249 while (failures <= max_repeats) {
250 if (m_device_api->getPositions(&m_file_descriptors.at(m_connected_devices.at(id)), id, positions) < 0) {
251 failures++;
252 continue;
253 }
254 break;
255 }
256 return failures;
257}
258
259int vpQbDevice::Impl::getSerialPortsAndDevices(const int &max_repeats)
260{
261 std::map<int, std::string> connected_devices;
262 std::array<char[255], 10> serial_ports;
263 int serial_ports_number = m_device_api->getSerialPorts(serial_ports);
264
265 for (size_t i = 0; i < static_cast<size_t>(serial_ports_number); i++) {
266 int failures = 0;
267 while (failures <= max_repeats) {
268 if (open(serial_ports.at(i)) != 0) {
269 failures++;
270 continue;
271 }
272 break;
273 }
274 if (failures >= max_repeats) {
275 continue;
276 }
277
278 // 'serial_protectors_' is not cleared because of the previously acquired lock, do not do it!
279#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
280 m_serial_protectors.insert(std::make_pair(serial_ports.at(i), std::make_unique<std::mutex>())); // never override
281#else
282 m_serial_protectors.insert(
283 std::make_pair(serial_ports.at(i), std::unique_ptr<std::mutex>(new std::mutex()))); // never override
284#endif
285
286 std::array<char, 255> devices;
287 int devices_number = m_device_api->getDeviceIds(&m_file_descriptors.at(serial_ports.at(i)), devices);
288 for (size_t j = 0; j < static_cast<size_t>(devices_number); j++) {
289
290 if (devices.at(j) == 120) {
291 continue; // ID 120 is reserved for dummy board which should not be considered as a connected device
292 }
293 // actually a std::map does not let same-id devices on distinct serial ports
294 connected_devices.insert(
295 std::make_pair(static_cast<int>(devices.at(j)), static_cast<std::string>(serial_ports.at(i))));
296 }
297 }
298
299 std::cout << "has found [" << connected_devices.size() << "] devices connected:" << std::endl;
300 for (auto const &device : connected_devices) {
301 std::cout << " - device [" << device.first << "] connected through [" << device.second << "]" << std::endl;
302 }
303
304 m_connected_devices = connected_devices;
305 return static_cast<int>(m_connected_devices.size());
306}
307
308bool vpQbDevice::Impl::init(const int &id)
309{
310 std::vector<int> encoder_resolutions;
311 std::vector<std::unique_lock<std::mutex> >
312 serial_locks; // need to lock on all the serial resources to scan for new ports/devices
313 for (auto const &mutex : m_serial_protectors) {
314 serial_locks.push_back(std::unique_lock<std::mutex>(*mutex.second));
315 }
316
317 // update connected devices
318 getSerialPortsAndDevices(m_max_repeats);
319
320 if (!isInConnectedSet(id) || !isReliable(isConnected(id, m_max_repeats), m_max_repeats)) {
321 std::cout << "fails while initializing device [" << id << "] because it is not connected." << std::endl;
322 return false;
323 }
324
325 std::vector<int> position_limits;
326
327 if (getParameters(id, position_limits, encoder_resolutions)) {
328 std::cout << "fails while initializing device [" << id
329 << "] because it requires 'USB' input mode and 'Position' control mode." << std::endl;
330 return false;
331 }
332
333 m_position_limits.resize(position_limits.size());
334 for (size_t i = 0; i < position_limits.size(); i++) {
335 m_position_limits[i] = static_cast<short int>(position_limits[i]);
336 }
337
338 std::string info;
339 int failures = getInfo(id, m_max_repeats, info);
340 if (!isReliable(failures, m_max_repeats)) {
341 std::cout << "has not initialized device [" << id << "] because it cannot get info." << std::endl;
342 return false;
343 }
344
345 std::string sep = "\n";
346 std::string current_limit = "Current limit:";
347 std::vector<std::string> subChain = vpIoTools::splitChain(info, sep);
348 bool current_max_found = false;
349 for (size_t i = 0; i < subChain.size(); i++) {
350 if (subChain[i].compare(0, current_limit.size(), current_limit) == 0) {
351 sep = ":";
352 std::vector<std::string> subChainLimit = vpIoTools::splitChain(subChain[i], sep);
353 m_current_max = std::atof(subChainLimit[1].c_str());
354 current_max_found = true;
355 break;
356 }
357 }
358 if (!current_max_found) {
359 std::cout << "has not initialized device [" << id << "] because it cannot get the max current." << std::endl;
360 return false;
361 }
362
363 failures = activate(id, m_max_repeats);
364 if (!isReliable(failures, m_max_repeats)) {
365 std::cout << "has not initialized device [" << id
366 << "] because it cannot activate its motors (please, check the motor positions)." << std::endl;
367 return false;
368 }
369
370 std::string serial_port = m_connected_devices.at(id);
371 std::cout << "Device [" + std::to_string(id) + "] connected on port [" << serial_port << "] initialization succeeds."
372 << std::endl;
373
374 return true;
375}
376
377int vpQbDevice::Impl::isActive(const int &id, const int &max_repeats, bool &status)
378{
379 // the API methods are called at most (i.e. very unlikely) 'max_repeats' times to guarantee the correct identification
380 // of a real fault in the communication
381 int failures = 0;
382 status = false;
383 while (failures <= max_repeats) {
384 if (!m_device_api->getStatus(&m_file_descriptors.at(m_connected_devices.at(id)), id, status)) {
385 failures++;
386 continue;
387 }
388 break;
389 }
390 return failures;
391}
392
393int vpQbDevice::Impl::isConnected(const int &id, const int &max_repeats)
394{
395 // the API methods are called at most (i.e. very unlikely) 'max_repeats' times to guarantee the correct identification
396 // of a real fault in the communication
397 int failures = 0;
398 while (failures <= max_repeats) {
399 if (!m_device_api->getStatus(&m_file_descriptors.at(m_connected_devices.at(id)), id)) {
400 failures++;
401 continue;
402 }
403 break;
404 }
405 return failures;
406}
407
408int vpQbDevice::Impl::open(const std::string &serial_port)
409{
410#if (defined(__APPLE__) && defined(__MACH__))
411 if (!std::regex_match(serial_port, std::regex("/dev/tty.usbserial-[[:digit:]]+"))) {
412 std::cout << "vpQbDevice fails while opening [" << serial_port
413 << "] because it does not match the expected pattern [/dev/tty.usbserial-*]." << std::endl;
414 return -1;
415 }
416#elif defined(__unix__) || defined(__unix)
417 if (!std::regex_match(serial_port, std::regex("/dev/ttyUSB[[:digit:]]+"))) {
418 std::cout << "vpQbDevice fails while opening [" << serial_port
419 << "] because it does not match the expected pattern [/dev/ttyUSB*]." << std::endl;
420 return -1;
421 }
422#elif defined(_WIN32)
423 if (!std::regex_match(serial_port, std::regex("COM[[:digit:]]+"))) {
424 std::cout << "vpQbDevice fails while opening [" << serial_port
425 << "] because it does not match the expected pattern [COM*]." << std::endl;
426 return -1;
427 }
428#endif
429
430 if (isInOpenMap(serial_port)) {
431 std::cout << "vpQbDevice already handles [" << serial_port << "]." << std::endl;
432 return 0; // no error: the communication is open anyway
433 }
434
435 m_device_api->open(&m_file_descriptors[serial_port], serial_port); // also create a pair in the map
436 if (m_file_descriptors.at(serial_port).file_handle == INVALID_HANDLE_VALUE) {
437 std::cout << "vpQbDevice fails while opening [" << serial_port << "] and sets errno [" << strerror(errno) << "]."
438 << std::endl;
439 // remove file descriptor entry
440 m_file_descriptors.erase(serial_port);
441 return -1;
442 }
443
444 std::cout << "Connect qb device to [" << serial_port << "]." << std::endl;
445 return 0;
446}
447
448int vpQbDevice::Impl::setCommandsAndWait(const int &id, const int &max_repeats, std::vector<short int> &commands)
449{
450 // the API methods are called at most (i.e. very unlikely) 'max_repeats' times to guarantee the correct identification
451 // of a real fault in the communication
452 int failures = 0;
453 commands.resize(2); // required by 'setCommandsAndWait()'
454 while (failures <= max_repeats) {
455 if (m_device_api->setCommandsAndWait(&m_file_descriptors.at(m_connected_devices.at(id)), id, commands) < 0) {
456 failures++;
457 continue;
458 }
459 break;
460 }
461 return failures;
462}
463
464int vpQbDevice::Impl::setCommandsAsync(const int &id, std::vector<short int> &commands)
465{
466 // qbhand sets only inputs.at(0), but setCommandsAsync expects two-element vector (ok for both qbhand and qbmove)
467 commands.resize(2); // required by 'setCommandsAsync()'
468 std::lock_guard<std::mutex> serial_lock(*m_serial_protectors.at(m_connected_devices.at(id)));
469 m_device_api->setCommandsAsync(&m_file_descriptors.at(m_connected_devices.at(id)), id, commands);
470 return 0; // note that this is a non reliable method
471}
472#endif // DOXYGEN_SHOULD_SKIP_THIS
473
478vpQbDevice::vpQbDevice() : m_impl(new Impl()), m_max_repeats(1), m_init_done(false) { setMaxRepeats(2); }
479
484vpQbDevice::~vpQbDevice() { delete m_impl; }
485
494int vpQbDevice::activate(const int &id, const bool &command, const int &max_repeats)
495{
496 return m_impl->activate(id, command, max_repeats);
497}
498
505int vpQbDevice::activate(const int &id, const int &max_repeats) { return m_impl->activate(id, max_repeats); }
506
513bool vpQbDevice::close(const std::string &serial_port) { return m_impl->close(serial_port); }
514
521int vpQbDevice::deactivate(const int &id, const int &max_repeats) { return m_impl->deactivate(id, max_repeats); }
522
526double vpQbDevice::getCurrentMax() const { return m_impl->getCurrentMax(); }
527
538int vpQbDevice::getCurrents(const int &id, const int &max_repeats, std::vector<short int> &currents)
539{
540 return m_impl->getCurrents(id, max_repeats, currents);
541}
542
551int vpQbDevice::getInfo(const int &id, const int &max_repeats, std::string &info)
552{
553 return m_impl->getInfo(id, max_repeats, info);
554}
555
570int vpQbDevice::getMeasurements(const int &id, const int &max_repeats, std::vector<short int> &currents,
571 std::vector<short int> &positions)
572{
573 return m_impl->getMeasurements(id, max_repeats, currents, positions);
574}
575
588int vpQbDevice::getParameters(const int &id, std::vector<int> &limits, std::vector<int> &resolutions)
589{
590 return m_impl->getParameters(id, limits, resolutions);
591}
592
596std::vector<short int> vpQbDevice::getPositionLimits() const { return m_impl->getPositionLimits(); }
597
609int vpQbDevice::getPositions(const int &id, const int &max_repeats, std::vector<short int> &positions)
610{
611 return m_impl->getPositions(id, max_repeats, positions);
612}
613
624int vpQbDevice::getSerialPortsAndDevices(const int &max_repeats)
625{
626 return m_impl->getSerialPortsAndDevices(max_repeats);
627}
628
636bool vpQbDevice::init(const int &id)
637{
638 bool ret = m_impl->init(id);
639
640 if (ret) {
641 m_init_done = true;
642 }
643 return ret;
644}
645
653int vpQbDevice::isActive(const int &id, const int &max_repeats, bool &status)
654{
655 return m_impl->isActive(id, max_repeats, status);
656}
657
664int vpQbDevice::isConnected(const int &id, const int &max_repeats) { return m_impl->isConnected(id, max_repeats); }
665
672bool vpQbDevice::isInConnectedSet(const int &id) { return m_impl->isInConnectedSet(id); }
673
680bool vpQbDevice::isInOpenMap(const std::string &serial_port) { return m_impl->isInOpenMap(serial_port); }
681
688bool vpQbDevice::isReliable(int const &failures, int const &max_repeats)
689{
690 return m_impl->isReliable(failures, max_repeats);
691}
692
700int vpQbDevice::open(const std::string &serial_port) { return m_impl->open(serial_port); }
701
711int vpQbDevice::setCommandsAndWait(const int &id, const int &max_repeats, std::vector<short int> &commands)
712{
713 return m_impl->setCommandsAndWait(id, max_repeats, commands);
714}
715
724int vpQbDevice::setCommandsAsync(const int &id, std::vector<short int> &commands)
725{
726 return m_impl->setCommandsAsync(id, commands);
727}
728
733void vpQbDevice::setMaxRepeats(const int &max_repeats)
734{
735 m_max_repeats = max_repeats;
736 m_impl->setMaxRepeats(m_max_repeats);
737}
738
739#endif
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
virtual int deactivate(const int &id, const int &max_repeats)
bool isReliable(int const &failures, int const &max_repeats)
virtual bool isInOpenMap(const std::string &serial_port)
double getCurrentMax() const
virtual int getSerialPortsAndDevices(const int &max_repeats)
virtual bool isInConnectedSet(const int &id)
virtual bool init(const int &id)
int isConnected(const int &id, const int &max_repeats)
int m_max_repeats
Max number of trials to send a command.
Definition vpQbDevice.h:112
virtual int getCurrents(const int &id, const int &max_repeats, std::vector< short int > &currents)
virtual bool close(const std::string &serial_port)
virtual int getMeasurements(const int &id, const int &max_repeats, std::vector< short int > &currents, std::vector< short int > &positions)
virtual int activate(const int &id, const bool &command, const int &max_repeats)
virtual int getParameters(const int &id, std::vector< int > &limits, std::vector< int > &resolutions)
std::vector< short int > getPositionLimits() const
virtual int getPositions(const int &id, const int &max_repeats, std::vector< short int > &positions)
virtual int getInfo(const int &id, const int &max_repeats, std::string &info)
void setMaxRepeats(const int &max_repeats)
virtual ~vpQbDevice()
virtual int setCommandsAndWait(const int &id, const int &max_repeats, std::vector< short int > &commands)
virtual int setCommandsAsync(const int &id, std::vector< short int > &commands)
virtual int isActive(const int &id, const int &max_repeats, bool &status)
virtual int open(const std::string &serial_port)
bool m_init_done
Flag used to indicate if the device is initialized.
Definition vpQbDevice.h:113