36#include <visp3/core/vpConfig.h>
37#ifdef VISP_HAVE_QBDEVICE
41#include <qb_device_driver.h>
43#include <visp3/core/vpIoTools.h>
44#include <visp3/robot/vpQbDevice.h>
46#ifndef DOXYGEN_SHOULD_SKIP_THIS
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.)
56 m_position_limits[0] = 0;
57 m_position_limits[1] = 19000;
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);
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); }
74 virtual bool close(
const std::string &serial_port);
76 virtual int deactivate(
const int &
id,
const int &max_repeats) {
return activate(
id,
false, max_repeats); }
78 inline double getCurrentMax()
const {
return m_current_max; }
80 virtual int getCurrents(
const int &
id,
const int &max_repeats, std::vector<short int> ¤ts);
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> ¤ts,
83 std::vector<short int> &positions);
84 virtual int getParameters(
const int &
id, std::vector<int> &limits, std::vector<int> &resolutions);
86 std::vector<short int> getPositionLimits()
const {
return m_position_limits; }
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);
93 virtual int isConnected(
const int &
id,
const int &max_repeats);
95 virtual bool isInConnectedSet(
const int &
id) {
return (m_connected_devices.count(
id) ?
true :
false); }
97 virtual bool isInOpenMap(
const std::string &serial_port)
99 return (m_file_descriptors.count(serial_port) ?
true :
false);
102 inline bool isReliable(
int const &failures,
int const &max_repeats)
104 return failures >= 0 && failures <= max_repeats;
106 virtual int open(
const std::string &serial_port);
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);
111 void setMaxRepeats(
const int &max_repeats) { m_max_repeats = max_repeats; }
114 std::map<std::string, std::unique_ptr<std::mutex> >
116 std::map<int, std::string> m_connected_devices;
119#if (defined(_WIN32) || defined(_WIN64))
120 std::unique_ptr<std::mutex> m_mutex_dummy;
122 std::vector<short int> m_position_limits;
123 std::shared_ptr<qb_device_driver::qbDeviceAPI> m_device_api;
124 std::map<std::string, comm_settings> m_file_descriptors;
126 double m_current_max;
129int vpQbDevice::Impl::activate(
const int &
id,
const bool &command,
const int &max_repeats)
131 std::string command_prefix = command ?
"" :
"de";
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;
144 std::cout <<
"Device [" <<
id <<
"] motors have been " << command_prefix <<
"activated!" << std::endl;
147 std::cout <<
"Device [" <<
id <<
"] motors were already " << command_prefix <<
"activated!" << std::endl;
151bool vpQbDevice::Impl::close(
const std::string &serial_port)
153 if (!isInOpenMap(serial_port)) {
154 std::cout <<
"has not handled [" << serial_port <<
"]." << std::endl;
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);
166 m_device_api->close(&m_file_descriptors.at(serial_port));
171 std::cout <<
"does not handle [" << serial_port <<
"] anymore." << std::endl;
175int vpQbDevice::Impl::getCurrents(
const int &
id,
const int &max_repeats, std::vector<short int> ¤ts)
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) {
192int vpQbDevice::Impl::getInfo(
const int &
id,
const int &max_repeats, std::string &info)
197 while (failures <= max_repeats) {
198 info = m_device_api->getInfo(&m_file_descriptors.at(m_connected_devices.at(
id)),
id);
208int vpQbDevice::Impl::getMeasurements(
const int &
id,
const int &max_repeats, std::vector<short int> ¤ts,
209 std::vector<short int> &positions)
216 std::vector<short int> measurements(5, 0);
217 while (failures <= max_repeats) {
218 if (m_device_api->getMeasurements(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, measurements) < 0) {
222 std::copy(measurements.begin(), measurements.begin() + 2, currents.begin());
223 std::copy(measurements.begin() + 2, measurements.end(), positions.begin());
229int vpQbDevice::Impl::getParameters(
const int &
id, std::vector<int> &limits, std::vector<int> &resolutions)
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()) {
242int vpQbDevice::Impl::getPositions(
const int &
id,
const int &max_repeats, std::vector<short int> &positions)
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) {
259int vpQbDevice::Impl::getSerialPortsAndDevices(
const int &max_repeats)
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);
265 for (
size_t i = 0; i < static_cast<size_t>(serial_ports_number); i++) {
267 while (failures <= max_repeats) {
268 if (open(serial_ports.at(i)) != 0) {
274 if (failures >= max_repeats) {
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>()));
282 m_serial_protectors.insert(
283 std::make_pair(serial_ports.at(i), std::unique_ptr<std::mutex>(
new std::mutex())));
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++) {
290 if (devices.at(j) == 120) {
294 connected_devices.insert(
295 std::make_pair(
static_cast<int>(devices.at(j)),
static_cast<std::string
>(serial_ports.at(i))));
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;
304 m_connected_devices = connected_devices;
305 return static_cast<int>(m_connected_devices.size());
308bool vpQbDevice::Impl::init(
const int &
id)
310 std::vector<int> encoder_resolutions;
311 std::vector<std::unique_lock<std::mutex> >
313 for (
auto const &mutex : m_serial_protectors) {
314 serial_locks.push_back(std::unique_lock<std::mutex>(*mutex.second));
318 getSerialPortsAndDevices(m_max_repeats);
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;
325 std::vector<int> position_limits;
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;
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]);
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;
345 std::string sep =
"\n";
346 std::string current_limit =
"Current limit:";
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) {
353 m_current_max = std::atof(subChainLimit[1].c_str());
354 current_max_found =
true;
358 if (!current_max_found) {
359 std::cout <<
"has not initialized device [" <<
id <<
"] because it cannot get the max current." << std::endl;
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;
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."
377int vpQbDevice::Impl::isActive(
const int &
id,
const int &max_repeats,
bool &status)
383 while (failures <= max_repeats) {
384 if (!m_device_api->getStatus(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, status)) {
393int vpQbDevice::Impl::isConnected(
const int &
id,
const int &max_repeats)
398 while (failures <= max_repeats) {
399 if (!m_device_api->getStatus(&m_file_descriptors.at(m_connected_devices.at(
id)),
id)) {
408int vpQbDevice::Impl::open(
const std::string &serial_port)
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;
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;
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;
430 if (isInOpenMap(serial_port)) {
431 std::cout <<
"vpQbDevice already handles [" << serial_port <<
"]." << std::endl;
435 m_device_api->open(&m_file_descriptors[serial_port], serial_port);
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) <<
"]."
440 m_file_descriptors.erase(serial_port);
444 std::cout <<
"Connect qb device to [" << serial_port <<
"]." << std::endl;
448int vpQbDevice::Impl::setCommandsAndWait(
const int &
id,
const int &max_repeats, std::vector<short int> &commands)
454 while (failures <= max_repeats) {
455 if (m_device_api->setCommandsAndWait(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, commands) < 0) {
464int vpQbDevice::Impl::setCommandsAsync(
const int &
id, std::vector<short int> &commands)
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);
496 return m_impl->activate(
id, command, max_repeats);
540 return m_impl->getCurrents(
id, max_repeats, currents);
553 return m_impl->getInfo(
id, max_repeats, info);
571 std::vector<short int> &positions)
573 return m_impl->getMeasurements(
id, max_repeats, currents, positions);
590 return m_impl->getParameters(
id, limits, resolutions);
611 return m_impl->getPositions(
id, max_repeats, positions);
626 return m_impl->getSerialPortsAndDevices(max_repeats);
638 bool ret = m_impl->init(
id);
655 return m_impl->isActive(
id, max_repeats, status);
690 return m_impl->isReliable(failures, max_repeats);
713 return m_impl->setCommandsAndWait(
id, max_repeats, commands);
726 return m_impl->setCommandsAsync(
id, commands);
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.
virtual int getCurrents(const int &id, const int &max_repeats, std::vector< short int > ¤ts)
virtual bool close(const std::string &serial_port)
virtual int getMeasurements(const int &id, const int &max_repeats, std::vector< short int > ¤ts, 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 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.