vpQbDevice.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2025 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Interface for the qb robotics devices.
 */
 
#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_QBDEVICE) && defined(VISP_HAVE_THREADS)
 
#include <regex>
 
#include <qb_device_driver.h>
 
#include <visp3/core/vpIoTools.h>
#include <visp3/robot/vpQbDevice.h>
 
BEGIN_VISP_NAMESPACE
#ifndef DOXYGEN_SHOULD_SKIP_THIS
class vpQbDevice::Impl
{
public:
  Impl() : Impl(std::make_shared<qb_device_driver::qbDeviceAPI>()) { }
  Impl(std::shared_ptr<qb_device_driver::qbDeviceAPI> device_api)
    : m_serial_protectors(), m_connected_devices(), m_position_limits(2), m_device_api(device_api),
    m_file_descriptors(), m_max_repeats(1), m_current_max(750.)
  {
    // Default values updated after a call to init()
    m_position_limits[0] = 0;
    m_position_limits[1] = 19000;
  }
 
  virtual ~Impl()
  {
    for (auto it = m_file_descriptors.begin(); it != m_file_descriptors.end();) {
      if (close(it->first)) {
        it = m_file_descriptors.erase(it);
      }
      else {
        ++it;
      }
    }
  }
 
  virtual int activate(const int &id, const bool &command, const int &max_repeats);
  virtual int activate(const int &id, const int &max_repeats) { return activate(id, true, max_repeats); }
 
  virtual bool close(const std::string &serial_port);
 
  virtual int deactivate(const int &id, const int &max_repeats) { return activate(id, false, max_repeats); }
 
  inline double getCurrentMax() const { return m_current_max; }
 
  virtual int getCurrents(const int &id, const int &max_repeats, std::vector<short int> &currents);
  virtual int getInfo(const int &id, const int &max_repeats, std::string &info);
  virtual int getMeasurements(const int &id, const int &max_repeats, std::vector<short int> &currents,
                              std::vector<short int> &positions);
  virtual int getParameters(const int &id, std::vector<int> &limits, std::vector<int> &resolutions);
 
  std::vector<short int> getPositionLimits() const { return m_position_limits; }
 
  virtual int getPositions(const int &id, const int &max_repeats, std::vector<short int> &positions);
  virtual int getSerialPortsAndDevices(const int &max_repeats);
  virtual bool init(const int &id);
  virtual int isActive(const int &id, const int &max_repeats, bool &status);
 
  virtual int isConnected(const int &id, const int &max_repeats);
 
  virtual bool isInConnectedSet(const int &id) { return (m_connected_devices.count(id) ? true : false); }
 
  virtual bool isInOpenMap(const std::string &serial_port)
  {
    return (m_file_descriptors.count(serial_port) ? true : false);
  }
 
  inline bool isReliable(int const &failures, int const &max_repeats)
  {
    return failures >= 0 && failures <= max_repeats;
  }
  virtual int open(const std::string &serial_port);
 
  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);
 
  void setMaxRepeats(const int &max_repeats) { m_max_repeats = max_repeats; }
 
public:
  std::map<std::string, std::unique_ptr<std::mutex> >
    m_serial_protectors; // only callbacks must lock the serial resources
  std::map<int, std::string> m_connected_devices;
 
protected:
#if (defined(_WIN32) || defined(_WIN64))
  std::unique_ptr<std::mutex> m_mutex_dummy; // FS: cannot build without this line with msvc
#endif
  std::vector<short int> m_position_limits; // min and max position values in ticks
  std::shared_ptr<qb_device_driver::qbDeviceAPI> m_device_api;
  std::map<std::string, comm_settings> m_file_descriptors;
  int m_max_repeats;
  double m_current_max;
};
 
int vpQbDevice::Impl::activate(const int &id, const bool &command, const int &max_repeats)
{
  std::string command_prefix = command ? "" : "de";
  bool status = false;
  int failures = 0;
 
  failures = isActive(id, max_repeats, status);
  if (status != command) {
    m_device_api->activate(&m_file_descriptors.at(m_connected_devices.at(id)), id, command);
    failures = std::max<int>(failures, isActive(id, max_repeats, status));
    if (status != command) {
      std::cout << "Device [" << id << "] fails on " << command_prefix << "activation." << std::endl;
      return -1;
    }
    std::cout << "Device [" << id << "] motors have been " << command_prefix << "activated!" << std::endl;
    return failures;
  }
  std::cout << "Device [" << id << "] motors were already " << command_prefix << "activated!" << std::endl;
  return failures;
}
 
bool vpQbDevice::Impl::close(const std::string &serial_port)
{
  if (!isInOpenMap(serial_port)) {
    std::cout << "has not handled [" << serial_port << "]." << std::endl;
    return false; // no error: the communication is close anyway
  }
 
  for (auto const &device : m_connected_devices) {
    if (device.second == serial_port) {
      deactivate(device.first, m_max_repeats);
      m_connected_devices.erase(device.first);
      break;
    }
  }
 
  m_device_api->close(&m_file_descriptors.at(serial_port));
 
  // Note that m_file_descriptors.erase(serial_port) is done in the destructor.
  // Cannot be done here since the iterator that is used in the destructor would be lost
 
  std::cout << "does not handle [" << serial_port << "] anymore." << std::endl;
  return true;
}
 
int vpQbDevice::Impl::getCurrents(const int &id, const int &max_repeats, std::vector<short int> &currents)
{
  // the API methods are called at most (i.e. very unlikely) 'max_repeats' times to guarantee the correct identification
  // of a real fault in the communication
  int failures = 0;
  currents.resize(2); // required by 'getCurrents()'
  std::lock_guard<std::mutex> serial_lock(*m_serial_protectors.at(m_connected_devices.at(id)));
  while (failures <= max_repeats) {
    if (m_device_api->getCurrents(&m_file_descriptors.at(m_connected_devices.at(id)), id, currents) < 0) {
      failures++;
      continue;
    }
    break;
  }
  return failures;
}
 
int vpQbDevice::Impl::getInfo(const int &id, const int &max_repeats, std::string &info)
{
  // the API methods are called at most (i.e. very unlikely) 'max_repeats' times to guarantee the correct identification
  // of a real fault in the communication
  int failures = 0;
  while (failures <= max_repeats) {
    info = m_device_api->getInfo(&m_file_descriptors.at(m_connected_devices.at(id)), id);
    if (info == "") {
      failures++;
      continue;
    }
    break;
  }
  return failures;
}
 
int vpQbDevice::Impl::getMeasurements(const int &id, const int &max_repeats, std::vector<short int> &currents,
                                      std::vector<short int> &positions)
{
  // the API methods are called at most (i.e. very unlikely) 'max_repeats' times to guarantee the correct identification
  // of a real fault in the communication
  int failures = 0;
  currents.resize(2);
  positions.resize(3);
  std::vector<short int> measurements(5, 0); // required by 'getMeasurements()'
  while (failures <= max_repeats) {
    if (m_device_api->getMeasurements(&m_file_descriptors.at(m_connected_devices.at(id)), id, measurements) < 0) {
      failures++;
      continue;
    }
    std::copy(measurements.begin(), measurements.begin() + 2, currents.begin());
    std::copy(measurements.begin() + 2, measurements.end(), positions.begin());
    break;
  }
  return failures;
}
 
int vpQbDevice::Impl::getParameters(const int &id, std::vector<int> &limits, std::vector<int> &resolutions)
{
  std::vector<int> input_mode = { -1 };
  std::vector<int> control_mode = { -1 };
  m_device_api->getParameters(&m_file_descriptors.at(m_connected_devices.at(id)), id, input_mode, control_mode,
                              resolutions, limits);
  if (!input_mode.front() && !control_mode.front()) { // both input and control modes equals 0 are required, i.e.
                                                      // respectively USB connected and position controlled
    return 0;
  }
  return -1;
}
 
int vpQbDevice::Impl::getPositions(const int &id, const int &max_repeats, std::vector<short int> &positions)
{
  // the API methods are called at most (i.e. very unlikely) 'max_repeats' times to guarantee the correct identification
  // of a real fault in the communication
  int failures = 0;
  positions.resize(3); // required by 'getPositions()'
  std::lock_guard<std::mutex> serial_lock(*m_serial_protectors.at(m_connected_devices.at(id)));
  while (failures <= max_repeats) {
    if (m_device_api->getPositions(&m_file_descriptors.at(m_connected_devices.at(id)), id, positions) < 0) {
      failures++;
      continue;
    }
    break;
  }
  return failures;
}
 
int vpQbDevice::Impl::getSerialPortsAndDevices(const int &max_repeats)
{
  std::map<int, std::string> connected_devices;
  std::array<char[255], 10> serial_ports;
  int serial_ports_number = m_device_api->getSerialPorts(serial_ports);
 
  for (size_t i = 0; i < static_cast<size_t>(serial_ports_number); i++) {
    int failures = 0;
    while (failures <= max_repeats) {
      if (open(serial_ports.at(i)) != 0) {
        failures++;
        continue;
      }
      break;
    }
    if (failures >= max_repeats) {
      continue;
    }
 
    // 'serial_protectors_' is not cleared because of the previously acquired lock, do not do it!
    // Check if std:c++14 or higher
#if ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
    m_serial_protectors.insert(std::make_pair(serial_ports.at(i), std::make_unique<std::mutex>())); // never override
#else
    m_serial_protectors.insert(
        std::make_pair(serial_ports.at(i), std::unique_ptr<std::mutex>(new std::mutex()))); // never override
#endif
 
    std::array<char, 255> devices;
    int devices_number = m_device_api->getDeviceIds(&m_file_descriptors.at(serial_ports.at(i)), devices);
    for (size_t j = 0; j < static_cast<size_t>(devices_number); j++) {
 
      if (devices.at(j) == 120) {
        continue; // ID 120 is reserved for dummy board which should not be considered as a connected device
      }
      // actually a std::map does not let same-id devices on distinct serial ports
      connected_devices.insert(
          std::make_pair(static_cast<int>(devices.at(j)), static_cast<std::string>(serial_ports.at(i))));
    }
  }
 
  std::cout << "has found [" << connected_devices.size() << "] devices connected:" << std::endl;
  for (auto const &device : connected_devices) {
    std::cout << " - device [" << device.first << "] connected through [" << device.second << "]" << std::endl;
  }
 
  m_connected_devices = connected_devices;
  return static_cast<int>(m_connected_devices.size());
}
 
bool vpQbDevice::Impl::init(const int &id)
{
  std::vector<int> encoder_resolutions;
  std::vector<std::unique_lock<std::mutex> >
    serial_locks; // need to lock on all the serial resources to scan for new ports/devices
  for (auto const &mutex : m_serial_protectors) {
    serial_locks.push_back(std::unique_lock<std::mutex>(*mutex.second));
  }
 
  // update connected devices
  getSerialPortsAndDevices(m_max_repeats);
 
  if (!isInConnectedSet(id) || !isReliable(isConnected(id, m_max_repeats), m_max_repeats)) {
    std::cout << "fails while initializing device [" << id << "] because it is not connected." << std::endl;
    return false;
  }
 
  std::vector<int> position_limits;
 
  if (getParameters(id, position_limits, encoder_resolutions)) {
    std::cout << "fails while initializing device [" << id
      << "] because it requires 'USB' input mode and 'Position' control mode." << std::endl;
    return false;
  }
 
  m_position_limits.resize(position_limits.size());
  for (size_t i = 0; i < position_limits.size(); i++) {
    m_position_limits[i] = static_cast<short int>(position_limits[i]);
  }
 
  std::string info;
  int failures = getInfo(id, m_max_repeats, info);
  if (!isReliable(failures, m_max_repeats)) {
    std::cout << "has not initialized device [" << id << "] because it cannot get info." << std::endl;
    return false;
  }
 
  std::string sep = "\n";
  std::string current_limit = "Current limit:";
  std::vector<std::string> subChain = vpIoTools::splitChain(info, sep);
  bool current_max_found = false;
  for (size_t i = 0; i < subChain.size(); i++) {
    if (subChain[i].compare(0, current_limit.size(), current_limit) == 0) {
      sep = ":";
      std::vector<std::string> subChainLimit = vpIoTools::splitChain(subChain[i], sep);
      m_current_max = std::atof(subChainLimit[1].c_str());
      current_max_found = true;
      break;
    }
  }
  if (!current_max_found) {
    std::cout << "has not initialized device [" << id << "] because it cannot get the max current." << std::endl;
    return false;
  }
 
  failures = activate(id, m_max_repeats);
  if (!isReliable(failures, m_max_repeats)) {
    std::cout << "has not initialized device [" << id
      << "] because it cannot activate its motors (please, check the motor positions)." << std::endl;
    return false;
  }
 
  std::string serial_port = m_connected_devices.at(id);
  std::cout << "Device [" + std::to_string(id) + "] connected on port [" << serial_port << "] initialization succeeds."
    << std::endl;
 
  return true;
}
 
int vpQbDevice::Impl::isActive(const int &id, const int &max_repeats, bool &status)
{
  // the API methods are called at most (i.e. very unlikely) 'max_repeats' times to guarantee the correct identification
  // of a real fault in the communication
  int failures = 0;
  status = false;
  while (failures <= max_repeats) {
    if (!m_device_api->getStatus(&m_file_descriptors.at(m_connected_devices.at(id)), id, status)) {
      failures++;
      continue;
    }
    break;
  }
  return failures;
}
 
int vpQbDevice::Impl::isConnected(const int &id, const int &max_repeats)
{
  // the API methods are called at most (i.e. very unlikely) 'max_repeats' times to guarantee the correct identification
  // of a real fault in the communication
  int failures = 0;
  while (failures <= max_repeats) {
    if (!m_device_api->getStatus(&m_file_descriptors.at(m_connected_devices.at(id)), id)) {
      failures++;
      continue;
    }
    break;
  }
  return failures;
}
 
int vpQbDevice::Impl::open(const std::string &serial_port)
{
#if (defined(__APPLE__) && defined(__MACH__))
  if (!std::regex_match(serial_port, std::regex("/dev/tty.usbserial-[[:digit:]]+"))) {
    std::cout << "vpQbDevice fails while opening [" << serial_port
      << "] because it does not match the expected pattern [/dev/tty.usbserial-*]." << std::endl;
    return -1;
  }
#elif defined(__unix__) || defined(__unix)
  if (!std::regex_match(serial_port, std::regex("/dev/ttyUSB[[:digit:]]+"))) {
    std::cout << "vpQbDevice fails while opening [" << serial_port
      << "] because it does not match the expected pattern [/dev/ttyUSB*]." << std::endl;
    return -1;
  }
#elif defined(_WIN32)
  if (!std::regex_match(serial_port, std::regex("COM[[:digit:]]+"))) {
    std::cout << "vpQbDevice fails while opening [" << serial_port
      << "] because it does not match the expected pattern [COM*]." << std::endl;
    return -1;
  }
#endif
 
  if (isInOpenMap(serial_port)) {
    std::cout << "vpQbDevice already handles [" << serial_port << "]." << std::endl;
    return 0; // no error: the communication is open anyway
  }
 
  m_device_api->open(&m_file_descriptors[serial_port], serial_port); // also create a pair in the map
  if (m_file_descriptors.at(serial_port).file_handle == INVALID_HANDLE_VALUE) {
    std::cout << "vpQbDevice fails while opening [" << serial_port << "] and sets errno [" << strerror(errno) << "]."
      << std::endl;
// remove file descriptor entry
    m_file_descriptors.erase(serial_port);
    return -1;
  }
 
  std::cout << "Connect qb device to [" << serial_port << "]." << std::endl;
  return 0;
}
 
int vpQbDevice::Impl::setCommandsAndWait(const int &id, const int &max_repeats, std::vector<short int> &commands)
{
  // the API methods are called at most (i.e. very unlikely) 'max_repeats' times to guarantee the correct identification
  // of a real fault in the communication
  int failures = 0;
  commands.resize(2); // required by 'setCommandsAndWait()'
  while (failures <= max_repeats) {
    if (m_device_api->setCommandsAndWait(&m_file_descriptors.at(m_connected_devices.at(id)), id, commands) < 0) {
      failures++;
      continue;
    }
    break;
  }
  return failures;
}
 
int vpQbDevice::Impl::setCommandsAsync(const int &id, std::vector<short int> &commands)
{
  // qbhand sets only inputs.at(0), but setCommandsAsync expects two-element vector (ok for both qbhand and qbmove)
  commands.resize(2); // required by 'setCommandsAsync()'
  std::lock_guard<std::mutex> serial_lock(*m_serial_protectors.at(m_connected_devices.at(id)));
  m_device_api->setCommandsAsync(&m_file_descriptors.at(m_connected_devices.at(id)), id, commands);
  return 0; // note that this is a non reliable method
}
#endif // DOXYGEN_SHOULD_SKIP_THIS

vpQbDevice::vpQbDevice() : m_impl(new Impl()), m_max_repeats(1), m_init_done(false) { setMaxRepeats(2); }

vpQbDevice::~vpQbDevice() { delete m_impl; }

int vpQbDevice::activate(const int &id, const bool &command, const int &max_repeats)
{
  return m_impl->activate(id, command, max_repeats);
}

int vpQbDevice::activate(const int &id, const int &max_repeats) { return m_impl->activate(id, max_repeats); }

bool vpQbDevice::close(const std::string &serial_port) { return m_impl->close(serial_port); }

int vpQbDevice::deactivate(const int &id, const int &max_repeats) { return m_impl->deactivate(id, max_repeats); }

double vpQbDevice::getCurrentMax() const { return m_impl->getCurrentMax(); }

int vpQbDevice::getCurrents(const int &id, const int &max_repeats, std::vector<short int> &currents)
{
  return m_impl->getCurrents(id, max_repeats, currents);
}

int vpQbDevice::getInfo(const int &id, const int &max_repeats, std::string &info)
{
  return m_impl->getInfo(id, max_repeats, info);
}

int vpQbDevice::getMeasurements(const int &id, const int &max_repeats, std::vector<short int> &currents,
                                std::vector<short int> &positions)
{
  return m_impl->getMeasurements(id, max_repeats, currents, positions);
}

int vpQbDevice::getParameters(const int &id, std::vector<int> &limits, std::vector<int> &resolutions)
{
  return m_impl->getParameters(id, limits, resolutions);
}

std::vector<short int> vpQbDevice::getPositionLimits() const { return m_impl->getPositionLimits(); }

int vpQbDevice::getPositions(const int &id, const int &max_repeats, std::vector<short int> &positions)
{
  return m_impl->getPositions(id, max_repeats, positions);
}

int vpQbDevice::getSerialPortsAndDevices(const int &max_repeats)
{
  return m_impl->getSerialPortsAndDevices(max_repeats);
}

bool vpQbDevice::init(const int &id)
{
  bool ret = m_impl->init(id);
 
  if (ret) {
    m_init_done = true;
  }
  return ret;
}

int vpQbDevice::isActive(const int &id, const int &max_repeats, bool &status)
{
  return m_impl->isActive(id, max_repeats, status);
}

int vpQbDevice::isConnected(const int &id, const int &max_repeats) { return m_impl->isConnected(id, max_repeats); }

bool vpQbDevice::isInConnectedSet(const int &id) { return m_impl->isInConnectedSet(id); }

bool vpQbDevice::isInOpenMap(const std::string &serial_port) { return m_impl->isInOpenMap(serial_port); }

bool vpQbDevice::isReliable(int const &failures, int const &max_repeats)
{
  return m_impl->isReliable(failures, max_repeats);
}

int vpQbDevice::open(const std::string &serial_port) { return m_impl->open(serial_port); }

int vpQbDevice::setCommandsAndWait(const int &id, const int &max_repeats, std::vector<short int> &commands)
{
  return m_impl->setCommandsAndWait(id, max_repeats, commands);
}

int vpQbDevice::setCommandsAsync(const int &id, std::vector<short int> &commands)
{
  return m_impl->setCommandsAsync(id, commands);
}

void vpQbDevice::setMaxRepeats(const int &max_repeats)
{
  m_max_repeats = max_repeats;
  m_impl->setMaxRepeats(m_max_repeats);
}
END_VISP_NAMESPACE
#endif