36 #include <visp3/core/vpConfig.h>
37 #if defined(VISP_HAVE_QBDEVICE) && defined(VISP_HAVE_THREADS)
41 #include <qb_device_driver.h>
43 #include <visp3/core/vpIoTools.h>
44 #include <visp3/robot/vpQbDevice.h>
46 #ifndef DOXYGEN_SHOULD_SKIP_THIS
47 class vpQbDevice::Impl
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),
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);
72 virtual int activate(
const int &
id,
const bool &command,
const int &max_repeats);
73 virtual int activate(
const int &
id,
const int &max_repeats) {
return activate(
id,
true, max_repeats); }
75 virtual bool close(
const std::string &serial_port);
77 virtual int deactivate(
const int &
id,
const int &max_repeats) {
return activate(
id,
false, max_repeats); }
81 virtual int getCurrents(
const int &
id,
const int &max_repeats, std::vector<short int> ¤ts);
82 virtual int getInfo(
const int &
id,
const int &max_repeats, std::string &info);
83 virtual int getMeasurements(
const int &
id,
const int &max_repeats, std::vector<short int> ¤ts,
84 std::vector<short int> &positions);
85 virtual int getParameters(
const int &
id, std::vector<int> &limits, std::vector<int> &resolutions);
89 virtual int getPositions(
const int &
id,
const int &max_repeats, std::vector<short int> &positions);
91 virtual bool init(
const int &
id);
92 virtual int isActive(
const int &
id,
const int &max_repeats,
bool &status);
94 virtual int isConnected(
const int &
id,
const int &max_repeats);
96 virtual bool isInConnectedSet(
const int &
id) {
return (m_connected_devices.count(
id) ?
true :
false); }
98 virtual bool isInOpenMap(
const std::string &serial_port)
100 return (m_file_descriptors.count(serial_port) ?
true :
false);
103 inline bool isReliable(
int const &failures,
int const &max_repeats)
105 return failures >= 0 && failures <= max_repeats;
107 virtual int open(
const std::string &serial_port);
109 virtual int setCommandsAndWait(
const int &
id,
const int &max_repeats, std::vector<short int> &commands);
110 virtual int setCommandsAsync(
const int &
id, std::vector<short int> &commands);
115 std::map<std::string, std::unique_ptr<std::mutex> >
117 std::map<int, std::string> m_connected_devices;
120 #if (defined(_WIN32) || defined(_WIN64))
121 std::unique_ptr<std::mutex> m_mutex_dummy;
123 std::vector<short int> m_position_limits;
124 std::shared_ptr<qb_device_driver::qbDeviceAPI> m_device_api;
125 std::map<std::string, comm_settings> m_file_descriptors;
127 double m_current_max;
130 int vpQbDevice::Impl::activate(
const int &
id,
const bool &command,
const int &max_repeats)
132 std::string command_prefix = command ?
"" :
"de";
136 failures = isActive(
id, max_repeats, status);
137 if (status != command) {
138 m_device_api->activate(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, command);
139 failures = std::max<int>(failures, isActive(
id, max_repeats, status));
140 if (status != command) {
141 std::cout <<
"Device [" <<
id <<
"] fails on " << command_prefix <<
"activation." << std::endl;
145 std::cout <<
"Device [" <<
id <<
"] motors have been " << command_prefix <<
"activated!" << std::endl;
148 std::cout <<
"Device [" <<
id <<
"] motors were already " << command_prefix <<
"activated!" << std::endl;
152 bool vpQbDevice::Impl::close(
const std::string &serial_port)
154 if (!isInOpenMap(serial_port)) {
155 std::cout <<
"has not handled [" << serial_port <<
"]." << std::endl;
159 for (
auto const &device : m_connected_devices) {
160 if (device.second == serial_port) {
161 deactivate(device.first, m_max_repeats);
162 m_connected_devices.erase(device.first);
167 m_device_api->close(&m_file_descriptors.at(serial_port));
172 std::cout <<
"does not handle [" << serial_port <<
"] anymore." << std::endl;
176 int vpQbDevice::Impl::getCurrents(
const int &
id,
const int &max_repeats, std::vector<short int> ¤ts)
182 std::lock_guard<std::mutex> serial_lock(*m_serial_protectors.at(m_connected_devices.at(
id)));
183 while (failures <= max_repeats) {
184 if (m_device_api->getCurrents(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, currents) < 0) {
193 int vpQbDevice::Impl::getInfo(
const int &
id,
const int &max_repeats, std::string &info)
198 while (failures <= max_repeats) {
199 info = m_device_api->getInfo(&m_file_descriptors.at(m_connected_devices.at(
id)),
id);
209 int vpQbDevice::Impl::getMeasurements(
const int &
id,
const int &max_repeats, std::vector<short int> ¤ts,
210 std::vector<short int> &positions)
217 std::vector<short int> measurements(5, 0);
218 while (failures <= max_repeats) {
219 if (m_device_api->getMeasurements(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, measurements) < 0) {
223 std::copy(measurements.begin(), measurements.begin() + 2, currents.begin());
224 std::copy(measurements.begin() + 2, measurements.end(), positions.begin());
230 int vpQbDevice::Impl::getParameters(
const int &
id, std::vector<int> &limits, std::vector<int> &resolutions)
232 std::vector<int> input_mode = { -1 };
233 std::vector<int> control_mode = { -1 };
234 m_device_api->getParameters(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, input_mode, control_mode,
235 resolutions, limits);
236 if (!input_mode.front() && !control_mode.front()) {
243 int vpQbDevice::Impl::getPositions(
const int &
id,
const int &max_repeats, std::vector<short int> &positions)
249 std::lock_guard<std::mutex> serial_lock(*m_serial_protectors.at(m_connected_devices.at(
id)));
250 while (failures <= max_repeats) {
251 if (m_device_api->getPositions(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, positions) < 0) {
260 int vpQbDevice::Impl::getSerialPortsAndDevices(
const int &max_repeats)
262 std::map<int, std::string> connected_devices;
263 std::array<char[255], 10> serial_ports;
264 int serial_ports_number = m_device_api->getSerialPorts(serial_ports);
266 for (
size_t i = 0; i < static_cast<size_t>(serial_ports_number); i++) {
268 while (failures <= max_repeats) {
269 if (open(serial_ports.at(i)) != 0) {
275 if (failures >= max_repeats) {
281 #if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
282 m_serial_protectors.insert(std::make_pair(serial_ports.at(i), std::make_unique<std::mutex>()));
284 m_serial_protectors.insert(
285 std::make_pair(serial_ports.at(i), std::unique_ptr<std::mutex>(
new std::mutex())));
288 std::array<char, 255> devices;
289 int devices_number = m_device_api->getDeviceIds(&m_file_descriptors.at(serial_ports.at(i)), devices);
290 for (
size_t j = 0; j < static_cast<size_t>(devices_number); j++) {
292 if (devices.at(j) == 120) {
296 connected_devices.insert(
297 std::make_pair(
static_cast<int>(devices.at(j)),
static_cast<std::string
>(serial_ports.at(i))));
301 std::cout <<
"has found [" << connected_devices.size() <<
"] devices connected:" << std::endl;
302 for (
auto const &device : connected_devices) {
303 std::cout <<
" - device [" << device.first <<
"] connected through [" << device.second <<
"]" << std::endl;
306 m_connected_devices = connected_devices;
307 return static_cast<int>(m_connected_devices.size());
310 bool vpQbDevice::Impl::init(
const int &
id)
312 std::vector<int> encoder_resolutions;
313 std::vector<std::unique_lock<std::mutex> >
315 for (
auto const &mutex : m_serial_protectors) {
316 serial_locks.push_back(std::unique_lock<std::mutex>(*mutex.second));
320 getSerialPortsAndDevices(m_max_repeats);
322 if (!isInConnectedSet(
id) || !isReliable(isConnected(
id, m_max_repeats), m_max_repeats)) {
323 std::cout <<
"fails while initializing device [" <<
id <<
"] because it is not connected." << std::endl;
327 std::vector<int> position_limits;
329 if (getParameters(
id, position_limits, encoder_resolutions)) {
330 std::cout <<
"fails while initializing device [" <<
id
331 <<
"] because it requires 'USB' input mode and 'Position' control mode." << std::endl;
335 m_position_limits.resize(position_limits.size());
336 for (
size_t i = 0; i < position_limits.size(); i++) {
337 m_position_limits[i] =
static_cast<short int>(position_limits[i]);
341 int failures = getInfo(
id, m_max_repeats, info);
342 if (!isReliable(failures, m_max_repeats)) {
343 std::cout <<
"has not initialized device [" <<
id <<
"] because it cannot get info." << std::endl;
347 std::string sep =
"\n";
348 std::string current_limit =
"Current limit:";
350 bool current_max_found =
false;
351 for (
size_t i = 0; i < subChain.size(); i++) {
352 if (subChain[i].compare(0, current_limit.size(), current_limit) == 0) {
355 m_current_max = std::atof(subChainLimit[1].c_str());
356 current_max_found =
true;
360 if (!current_max_found) {
361 std::cout <<
"has not initialized device [" <<
id <<
"] because it cannot get the max current." << std::endl;
365 failures = activate(
id, m_max_repeats);
366 if (!isReliable(failures, m_max_repeats)) {
367 std::cout <<
"has not initialized device [" <<
id
368 <<
"] because it cannot activate its motors (please, check the motor positions)." << std::endl;
372 std::string serial_port = m_connected_devices.at(
id);
373 std::cout <<
"Device [" + std::to_string(
id) +
"] connected on port [" << serial_port <<
"] initialization succeeds."
379 int vpQbDevice::Impl::isActive(
const int &
id,
const int &max_repeats,
bool &status)
385 while (failures <= max_repeats) {
386 if (!m_device_api->getStatus(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, status)) {
395 int vpQbDevice::Impl::isConnected(
const int &
id,
const int &max_repeats)
400 while (failures <= max_repeats) {
401 if (!m_device_api->getStatus(&m_file_descriptors.at(m_connected_devices.at(
id)),
id)) {
410 int vpQbDevice::Impl::open(
const std::string &serial_port)
412 #if (defined(__APPLE__) && defined(__MACH__))
413 if (!std::regex_match(serial_port, std::regex(
"/dev/tty.usbserial-[[:digit:]]+"))) {
414 std::cout <<
"vpQbDevice fails while opening [" << serial_port
415 <<
"] because it does not match the expected pattern [/dev/tty.usbserial-*]." << std::endl;
418 #elif defined(__unix__) || defined(__unix)
419 if (!std::regex_match(serial_port, std::regex(
"/dev/ttyUSB[[:digit:]]+"))) {
420 std::cout <<
"vpQbDevice fails while opening [" << serial_port
421 <<
"] because it does not match the expected pattern [/dev/ttyUSB*]." << std::endl;
424 #elif defined(_WIN32)
425 if (!std::regex_match(serial_port, std::regex(
"COM[[:digit:]]+"))) {
426 std::cout <<
"vpQbDevice fails while opening [" << serial_port
427 <<
"] because it does not match the expected pattern [COM*]." << std::endl;
432 if (isInOpenMap(serial_port)) {
433 std::cout <<
"vpQbDevice already handles [" << serial_port <<
"]." << std::endl;
437 m_device_api->open(&m_file_descriptors[serial_port], serial_port);
438 if (m_file_descriptors.at(serial_port).file_handle == INVALID_HANDLE_VALUE) {
439 std::cout <<
"vpQbDevice fails while opening [" << serial_port <<
"] and sets errno [" << strerror(errno) <<
"]."
442 m_file_descriptors.erase(serial_port);
446 std::cout <<
"Connect qb device to [" << serial_port <<
"]." << std::endl;
450 int vpQbDevice::Impl::setCommandsAndWait(
const int &
id,
const int &max_repeats, std::vector<short int> &commands)
456 while (failures <= max_repeats) {
457 if (m_device_api->setCommandsAndWait(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, commands) < 0) {
466 int vpQbDevice::Impl::setCommandsAsync(
const int &
id, std::vector<short int> &commands)
470 std::lock_guard<std::mutex> serial_lock(*m_serial_protectors.at(m_connected_devices.at(
id)));
471 m_device_api->setCommandsAsync(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, commands);
498 return m_impl->activate(
id, command, max_repeats);
542 return m_impl->getCurrents(
id, max_repeats, currents);
555 return m_impl->getInfo(
id, max_repeats, info);
573 std::vector<short int> &positions)
575 return m_impl->getMeasurements(
id, max_repeats, currents, positions);
592 return m_impl->getParameters(
id, limits, resolutions);
613 return m_impl->getPositions(
id, max_repeats, positions);
628 return m_impl->getSerialPortsAndDevices(max_repeats);
640 bool ret = m_impl->init(
id);
657 return m_impl->isActive(
id, max_repeats, status);
692 return m_impl->isReliable(failures, max_repeats);
715 return m_impl->setCommandsAndWait(
id, max_repeats, commands);
728 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.