39 #include <visp3/core/vpConfig.h>
40 #ifdef VISP_HAVE_QBDEVICE
44 #include <qb_device_driver.h>
46 #include <visp3/core/vpIoTools.h>
47 #include <visp3/robot/vpQbDevice.h>
49 #ifndef DOXYGEN_SHOULD_SKIP_THIS
50 class vpQbDevice::Impl
53 Impl() : Impl(std::make_shared<qb_device_driver::qbDeviceAPI>()) {}
54 Impl(std::shared_ptr<qb_device_driver::qbDeviceAPI> device_api)
55 : m_serial_protectors(), m_connected_devices(), m_position_limits(2), m_device_api(device_api),
59 m_position_limits[0] = 0;
60 m_position_limits[1] = 19000;
65 for (
auto it = m_file_descriptors.begin(); it != m_file_descriptors.end();) {
66 if (
close(it->first)) {
67 it = m_file_descriptors.erase(it);
74 virtual int activate(
const int &
id,
const bool &command,
const int &max_repeats);
75 virtual int activate(
const int &
id,
const int &max_repeats) {
return activate(
id,
true, max_repeats); }
77 virtual bool close(
const std::string &serial_port);
79 virtual int deactivate(
const int &
id,
const int &max_repeats) {
return activate(
id,
false, max_repeats); }
83 virtual int getCurrents(
const int &
id,
const int &max_repeats, std::vector<short int> ¤ts);
84 virtual int getInfo(
const int &
id,
const int &max_repeats, std::string &info);
85 virtual int getMeasurements(
const int &
id,
const int &max_repeats, std::vector<short int> ¤ts,
86 std::vector<short int> &positions);
87 virtual int getParameters(
const int &
id, std::vector<int> &limits, std::vector<int> &resolutions);
91 virtual int getPositions(
const int &
id,
const int &max_repeats, std::vector<short int> &positions);
93 virtual bool init(
const int &
id);
94 virtual int isActive(
const int &
id,
const int &max_repeats,
bool &status);
96 virtual int isConnected(
const int &
id,
const int &max_repeats);
98 virtual bool isInConnectedSet(
const int &
id) {
return (m_connected_devices.count(
id) ?
true :
false); }
100 virtual bool isInOpenMap(
const std::string &serial_port)
102 return (m_file_descriptors.count(serial_port) ?
true :
false);
105 inline bool isReliable(
int const &failures,
int const &max_repeats)
107 return failures >= 0 && failures <= max_repeats;
109 virtual int open(
const std::string &serial_port);
111 virtual int setCommandsAndWait(
const int &
id,
const int &max_repeats, std::vector<short int> &commands);
112 virtual int setCommandsAsync(
const int &
id, std::vector<short int> &commands);
117 std::map<std::string, std::unique_ptr<std::mutex> >
119 std::map<int, std::string> m_connected_devices;
122 #if (defined(_WIN32) || defined(_WIN64))
123 std::unique_ptr<std::mutex> m_mutex_dummy;
125 std::vector<short int> m_position_limits;
126 std::shared_ptr<qb_device_driver::qbDeviceAPI> m_device_api;
127 std::map<std::string, comm_settings> m_file_descriptors;
129 double m_current_max;
132 int vpQbDevice::Impl::activate(
const int &
id,
const bool &command,
const int &max_repeats)
134 std::string command_prefix = command ?
"" :
"de";
138 failures = isActive(
id, max_repeats, status);
139 if (status != command) {
140 m_device_api->activate(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, command);
141 failures = std::max(failures, isActive(
id, max_repeats, status));
142 if (status != command) {
143 std::cout <<
"Device [" <<
id <<
"] fails on " << command_prefix <<
"activation." << std::endl;
147 std::cout <<
"Device [" <<
id <<
"] motors have been " << command_prefix <<
"activated!" << std::endl;
150 std::cout <<
"Device [" <<
id <<
"] motors were already " << command_prefix <<
"activated!" << std::endl;
154 bool vpQbDevice::Impl::close(
const std::string &serial_port)
156 if (!isInOpenMap(serial_port)) {
157 std::cout <<
"has not handled [" << serial_port <<
"]." << std::endl;
161 for (
auto const &device : m_connected_devices) {
162 if (device.second == serial_port) {
163 deactivate(device.first, m_max_repeats);
164 m_connected_devices.erase(device.first);
169 m_device_api->close(&m_file_descriptors.at(serial_port));
174 std::cout <<
"does not handle [" << serial_port <<
"] anymore." << std::endl;
178 int vpQbDevice::Impl::getCurrents(
const int &
id,
const int &max_repeats, std::vector<short int> ¤ts)
184 std::lock_guard<std::mutex> serial_lock(*m_serial_protectors.at(m_connected_devices.at(
id)));
185 while (failures <= max_repeats) {
186 if (m_device_api->getCurrents(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, currents) < 0) {
195 int vpQbDevice::Impl::getInfo(
const int &
id,
const int &max_repeats, std::string &info)
200 while (failures <= max_repeats) {
201 info = m_device_api->getInfo(&m_file_descriptors.at(m_connected_devices.at(
id)),
id);
211 int vpQbDevice::Impl::getMeasurements(
const int &
id,
const int &max_repeats, std::vector<short int> ¤ts,
212 std::vector<short int> &positions)
219 std::vector<short int> measurements(5, 0);
220 while (failures <= max_repeats) {
221 if (m_device_api->getMeasurements(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, measurements) < 0) {
225 std::copy(measurements.begin(), measurements.begin() + 2, currents.begin());
226 std::copy(measurements.begin() + 2, measurements.end(), positions.begin());
232 int vpQbDevice::Impl::getParameters(
const int &
id, std::vector<int> &limits, std::vector<int> &resolutions)
234 std::vector<int> input_mode = {-1};
235 std::vector<int> control_mode = {-1};
236 m_device_api->getParameters(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, input_mode, control_mode,
237 resolutions, limits);
238 if (!input_mode.front() && !control_mode.front()) {
245 int vpQbDevice::Impl::getPositions(
const int &
id,
const int &max_repeats, std::vector<short int> &positions)
251 std::lock_guard<std::mutex> serial_lock(*m_serial_protectors.at(m_connected_devices.at(
id)));
252 while (failures <= max_repeats) {
253 if (m_device_api->getPositions(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, positions) < 0) {
262 int vpQbDevice::Impl::getSerialPortsAndDevices(
const int &max_repeats)
264 std::map<int, std::string> connected_devices;
265 std::array<char[255], 10> serial_ports;
266 int serial_ports_number = m_device_api->getSerialPorts(serial_ports);
268 for (
size_t i = 0; i < static_cast<size_t>(serial_ports_number); i++) {
270 while (failures <= max_repeats) {
271 if (open(serial_ports.at(i)) != 0) {
277 if (failures >= max_repeats) {
282 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_14)
283 m_serial_protectors.insert(std::make_pair(serial_ports.at(i), std::make_unique<std::mutex>()));
285 m_serial_protectors.insert(
286 std::make_pair(serial_ports.at(i), std::unique_ptr<std::mutex>(
new std::mutex())));
289 std::array<char, 255> devices;
290 int devices_number = m_device_api->getDeviceIds(&m_file_descriptors.at(serial_ports.at(i)), devices);
291 for (
size_t j = 0; j < static_cast<size_t>(devices_number); j++) {
293 if (devices.at(j) == 120) {
297 connected_devices.insert(
298 std::make_pair(
static_cast<int>(devices.at(j)),
static_cast<std::string
>(serial_ports.at(i))));
302 std::cout <<
"has found [" << connected_devices.size() <<
"] devices connected:" << std::endl;
303 for (
auto const &device : connected_devices) {
304 std::cout <<
" - device [" << device.first <<
"] connected through [" << device.second <<
"]" << std::endl;
307 m_connected_devices = connected_devices;
308 return static_cast<int>(m_connected_devices.size());
311 bool vpQbDevice::Impl::init(
const int &
id)
313 std::vector<int> encoder_resolutions;
314 std::vector<std::unique_lock<std::mutex> >
316 for (
auto const &mutex : m_serial_protectors) {
317 serial_locks.push_back(std::unique_lock<std::mutex>(*mutex.second));
321 getSerialPortsAndDevices(m_max_repeats);
323 if (!isInConnectedSet(
id) || !isReliable(isConnected(
id, m_max_repeats), m_max_repeats)) {
324 std::cout <<
"fails while initializing device [" <<
id <<
"] because it is not connected." << std::endl;
328 std::vector<int> position_limits;
330 if (getParameters(
id, position_limits, encoder_resolutions)) {
331 std::cout <<
"fails while initializing device [" <<
id
332 <<
"] because it requires 'USB' input mode and 'Position' control mode." << std::endl;
336 m_position_limits.resize(position_limits.size());
337 for (
size_t i = 0; i < position_limits.size(); i++) {
338 m_position_limits[i] =
static_cast<short int>(position_limits[i]);
342 int failures = getInfo(
id, m_max_repeats, info);
343 if (!isReliable(failures, m_max_repeats)) {
344 std::cout <<
"has not initialized device [" <<
id <<
"] because it cannot get info." << std::endl;
348 std::string sep =
"\n";
349 std::string current_limit =
"Current limit:";
351 bool current_max_found =
false;
352 for (
size_t i = 0; i < subChain.size(); i++) {
353 if (subChain[i].compare(0, current_limit.size(), current_limit) == 0) {
356 m_current_max = std::atof(subChainLimit[1].c_str());
357 current_max_found =
true;
361 if (!current_max_found) {
362 std::cout <<
"has not initialized device [" <<
id <<
"] because it cannot get the max current." << std::endl;
366 failures = activate(
id, m_max_repeats);
367 if (!isReliable(failures, m_max_repeats)) {
368 std::cout <<
"has not initialized device [" <<
id
369 <<
"] because it cannot activate its motors (please, check the motor positions)." << std::endl;
373 std::string serial_port = m_connected_devices.at(
id);
374 std::cout <<
"Device [" + std::to_string(
id) +
"] connected on port [" << serial_port <<
"] initialization succeeds."
380 int vpQbDevice::Impl::isActive(
const int &
id,
const int &max_repeats,
bool &status)
386 while (failures <= max_repeats) {
387 if (!m_device_api->getStatus(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, status)) {
396 int vpQbDevice::Impl::isConnected(
const int &
id,
const int &max_repeats)
401 while (failures <= max_repeats) {
402 if (!m_device_api->getStatus(&m_file_descriptors.at(m_connected_devices.at(
id)),
id)) {
411 int vpQbDevice::Impl::open(
const std::string &serial_port)
413 #if (defined(__APPLE__) && defined(__MACH__))
414 if (!std::regex_match(serial_port, std::regex(
"/dev/tty.usbserial-[[:digit:]]+"))) {
415 std::cout <<
"vpQbDevice fails while opening [" << serial_port
416 <<
"] because it does not match the expected pattern [/dev/tty.usbserial-*]." << std::endl;
419 #elif defined(__unix__) || defined(__unix)
420 if (!std::regex_match(serial_port, std::regex(
"/dev/ttyUSB[[:digit:]]+"))) {
421 std::cout <<
"vpQbDevice fails while opening [" << serial_port
422 <<
"] because it does not match the expected pattern [/dev/ttyUSB*]." << std::endl;
425 #elif defined(_WIN32)
426 if (!std::regex_match(serial_port, std::regex(
"COM[[:digit:]]+"))) {
427 std::cout <<
"vpQbDevice fails while opening [" << serial_port
428 <<
"] because it does not match the expected pattern [COM*]." << std::endl;
433 if (isInOpenMap(serial_port)) {
434 std::cout <<
"vpQbDevice already handles [" << serial_port <<
"]." << std::endl;
438 m_device_api->open(&m_file_descriptors[serial_port], serial_port);
439 if (m_file_descriptors.at(serial_port).file_handle == INVALID_HANDLE_VALUE) {
440 std::cout <<
"vpQbDevice fails while opening [" << serial_port <<
"] and sets errno [" << strerror(errno) <<
"]."
443 m_file_descriptors.erase(serial_port);
447 std::cout <<
"Connect qb device to [" << serial_port <<
"]." << std::endl;
451 int vpQbDevice::Impl::setCommandsAndWait(
const int &
id,
const int &max_repeats, std::vector<short int> &commands)
457 while (failures <= max_repeats) {
458 if (m_device_api->setCommandsAndWait(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, commands) < 0) {
467 int vpQbDevice::Impl::setCommandsAsync(
const int &
id, std::vector<short int> &commands)
471 std::lock_guard<std::mutex> serial_lock(*m_serial_protectors.at(m_connected_devices.at(
id)));
472 m_device_api->setCommandsAsync(&m_file_descriptors.at(m_connected_devices.at(
id)),
id, commands);
499 return m_impl->activate(
id, command, max_repeats);
543 return m_impl->getCurrents(
id, max_repeats, currents);
556 return m_impl->getInfo(
id, max_repeats, info);
574 std::vector<short int> &positions)
576 return m_impl->getMeasurements(
id, max_repeats, currents, positions);
593 return m_impl->getParameters(
id, limits, resolutions);
614 return m_impl->getPositions(
id, max_repeats, positions);
629 return m_impl->getSerialPortsAndDevices(max_repeats);
641 bool ret = m_impl->init(
id);
658 return m_impl->isActive(
id, max_repeats, status);
693 return m_impl->isReliable(failures, max_repeats);
716 return m_impl->setCommandsAndWait(
id, max_repeats, commands);
729 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.