Visual Servoing Platform  version 3.6.1 under development (2024-03-29)
vpServolens.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 Servolens lens attached to the camera fixed on the
33  * Afma4 robot.
34  *
35 *****************************************************************************/
36 
46 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
47 
48 #include <fcntl.h>
49 #include <stdio.h>
50 #include <stdlib.h>
51 #include <string.h>
52 #include <sys/stat.h>
53 #include <sys/types.h>
54 #include <termios.h>
55 #include <unistd.h>
56 
57 #include <visp3/core/vpDebug.h>
58 #include <visp3/core/vpTime.h>
59 #include <visp3/robot/vpRobotException.h>
60 #include <visp3/robot/vpServolens.h>
61 
68 vpServolens::vpServolens() : remfd(0), isinit(false) {}
69 
78 vpServolens::vpServolens(const char *port) : remfd(0), isinit(false) { this->open(port); }
79 
88 
103 void vpServolens::open(const char *port)
104 {
105  if (!isinit) {
106  struct termios info;
107 
108  printf("\nOpen the Servolens serial port \"%s\"\n", port);
109 
110  if ((this->remfd = ::open(port, O_RDWR | O_NONBLOCK)) < 0) {
111  vpERROR_TRACE("Cannot open Servolens serial port.");
112  throw vpRobotException(vpRobotException::communicationError, "Cannot open Servolens serial port.");
113  }
114 
115  // Lecture des parametres courants de la liaison serie.
116  if (tcgetattr(this->remfd, &info) < 0) {
117  ::close(this->remfd);
118  vpERROR_TRACE("Error using TCGETS in ioctl.");
119  throw vpRobotException(vpRobotException::communicationError, "Error using TCGETS in ioctl");
120  }
121 
122  //
123  // Configuration de la liaison serie:
124  // 9600 bauds, 1 bit de stop, parite paire, 7 bits de donnee
125  //
126 
127  // Traitement sur les caracteres recus
128  info.c_iflag = 0;
129  info.c_iflag |= INLCR;
130 
131  // Traitement sur les caracteres envoyes sur la RS232.
132  info.c_oflag = 0; // idem
133 
134  // Traitement des lignes
135  info.c_lflag = 0;
136 
137  // Controle materiel de la liaison
138  info.c_cflag = 0;
139  info.c_cflag |= CREAD; // Validation reception
140  info.c_cflag |= B9600 | CS7 | PARENB; // 9600 baus, 7 data, parite paire
141 
142  // Caracteres immediatement disponibles.
143  // info.c_cc[VMIN] = 1;
144  // info.c_cc[VTIME] = 0;
145 
146  if (tcsetattr(this->remfd, TCSANOW, &info) < 0) {
147  ::close(this->remfd);
148  vpERROR_TRACE("Error using TCGETS in ioctl.");
149  throw vpRobotException(vpRobotException::communicationError, "Error using TCGETS in ioctl");
150  }
151 
152  // Supprime tous les caracteres recus mais non encore lus par read()
153  tcflush(this->remfd, TCIFLUSH);
154 
155  isinit = true;
156 
157  this->init();
158 
159  // Try to get the position of the zoom to check if the lens is really
160  // connected
161  unsigned int izoom;
162  if (this->getPosition(vpServolens::ZOOM, izoom) == false) {
163  vpERROR_TRACE("Cannot dial with the servolens. Check if the serial "
164  "link is connected.");
165  throw vpRobotException(vpRobotException::communicationError, "Cannot dial with the servolens. Check if the "
166  "serial link is connected.");
167  }
168  }
169 }
170 
176 {
177  if (isinit) {
178  printf("\nClose the serial connection with Servolens\n");
179  ::close(this->remfd);
180  isinit = false;
181  }
182 }
183 
190 void vpServolens::reset() const
191 {
192  if (!isinit) {
193  vpERROR_TRACE("Cannot dial with Servolens.");
194  throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
195  }
196  std::string cmd;
197 
198  /* suppression de l'echo */
199  cmd = "SE1";
200  this->write(cmd.c_str());
201 
202  /* initialisation de l'objectif, idem qu'a la mise sous tension */
203  cmd = "SR0";
204  this->write(cmd.c_str());
205 
206  vpTime::wait(25000);
207 
208  this->wait();
209 
210  /* suppression de l'echo */
211  cmd = "SE0";
212  this->write(cmd.c_str());
213 
214  /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
215  cmd = "VW0";
216  this->write(cmd.c_str());
217 }
226 void vpServolens::init() const
227 {
228  if (!isinit) {
229  vpERROR_TRACE("Cannot dial with Servolens.");
230  throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
231  }
232 
233  std::string cmd;
234 
235  /* suppression de l'echo */
236  cmd = "SE0";
237  this->write(cmd.c_str());
238 
239  /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
240  cmd = "VW0";
241  this->write(cmd.c_str());
242 
243  /* L'experience montre qu'une petite tempo est utile. */
244  vpTime::wait(500);
245 }
246 
257 void vpServolens::enablePrompt(bool active) const
258 {
259  if (!isinit) {
260  vpERROR_TRACE("Cannot dial with Servolens.");
261  throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
262  }
263  std::string cmd;
264 
265  /* suppression de l'echo */
266  if (active == true)
267  cmd = "SE1";
268  else
269  cmd = "SE0";
270 
271  this->write(cmd.c_str());
272 }
273 
283 {
284  if (!isinit) {
285  vpERROR_TRACE("Cannot dial with Servolens.");
286  throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
287  }
288  std::string cmd;
289 
290  switch (controller) {
291  case AUTO:
292  /* Valide l'incrustation de la fenetre sur l'ecran du moniteur */
293  cmd = "VW1";
294  this->write(cmd.c_str());
295  break;
296  case CONTROLLED:
297  /* nettoyage : mot d'etat vide 0000 */
298  cmd = "SX0842";
299  this->write(cmd.c_str());
300  /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
301  cmd = "VW0";
302  this->write(cmd.c_str());
303  break;
304  case RELEASED:
305  cmd = "SX1084";
306  this->write(cmd.c_str());
307  /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
308  cmd = "VW0";
309  this->write(cmd.c_str());
310  break;
311  }
312 }
313 
320 void vpServolens::setAutoIris(bool enable) const
321 {
322  if (!isinit) {
323  vpERROR_TRACE("Cannot dial with Servolens.");
324  throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
325  }
326  std::string cmd;
327 
328  if (enable)
329  cmd = "DA1";
330  else
331  cmd = "DA0";
332 
333  this->write(cmd.c_str());
334 }
335 
346 void vpServolens::setPosition(vpServoType servo, unsigned int position) const
347 {
348  if (!isinit) {
349  vpERROR_TRACE("Cannot dial with Servolens.");
350  throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
351  }
352  std::stringstream command;
353 
354  /* attente du prompt pour envoyer une commande */
355  /*
356  printf("attente prompt\n");
357  this->wait();
358  */
359 
360 #ifdef FINSERVO
361  /* envoie des commandes pour qu'en fin de mouvement servolens renvoie */
362  /* une commande de fin de mouvement (ex: ZF, FF, DF). */
363  this->enableCommandComplete();
364 #endif /* FINSERVO */
365 
366  // 08/08/00 Fabien S. - Correction de la consigne demandee
367  // pour prendre en compte l'erreur entre la consigne demandee
368  // et la consigne mesuree.
369  // A la consigne du zoom on retranche 1.
370  // A la consigne du focus on ajoute 1.
371  // A la consigne du iris on ajoute 1.
372  switch (servo) {
373  case ZOOM:
374  // printf("zoom demande: %d ", position);
375  position--;
376  if (position < ZOOM_MIN)
377  position = ZOOM_MIN;
378  // printf("zoom corrige: %d \n", position);
379  break;
380  case FOCUS:
381  // printf("focus demande: %d ", position);
382  position++;
383  if (position > FOCUS_MAX)
384  position = FOCUS_MAX;
385  // printf("focus corrige: %d \n", position);
386  break;
387  case IRIS:
388  // printf("iris demande: %d ", position);
389  position++;
390  if (position > IRIS_MAX)
391  position = IRIS_MAX;
392  // printf("iris corrige: %s \n", position);
393  break;
394  }
395 
396  /* commande a envoyer aux servomoteurs */
397  switch (servo) {
398  case ZOOM:
399  command << "ZD" << position;
400  break;
401  case FOCUS:
402  command << "FD" << position;
403  break;
404  case IRIS:
405  command << "DD" << position;
406  break;
407  }
408 /* envoie de la commande */
409 #ifdef PRINT
410  printf("\ncommand: %s", command.str());
411 #endif
412 
413  this->write(command.str().c_str());
414 
415 #ifdef FINSERVO
416  /* on attend la fin du mouvement des objectifs */
417  this->wait(servo); /* on attend les codes ZF, FF, DF */
418 #endif
419 }
420 
431 bool vpServolens::getPosition(vpServoType servo, unsigned int &position) const
432 {
433  if (!isinit) {
434  vpERROR_TRACE("Cannot dial with Servolens.");
435  throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
436  }
437  std::string cmd;
438  char posit[10], *pt_posit;
439  char c;
440  short fin_lect_posit; /* indique si on a lu la position du servo-moteur */
441  short posit_car; /* donne la position du caractere lu */
442  short lecture_posit_en_cours; /* indique si on lit la position courante */
443 
444  /* attente du prompt pour envoyer une commande */
445  /*
446  this->wait();
447  */
448  pt_posit = posit;
449 
450  /* envoie des commandes pour obtenir la position des servo-moteurs. */
451  switch (servo) {
452  case ZOOM:
453  cmd = "ZD?";
454  break;
455  case FOCUS:
456  cmd = "FD?";
457  break;
458  case IRIS:
459  cmd = "DD?";
460  break;
461  default:
462  break;
463  }
464  /* envoie de la commande */
465  // printf("\ncommande: %s", commande);
466 
467  this->write(cmd.c_str());
468 
469  /* on cherche a lire la position du servo-moteur */
470  /* Servolens renvoie une chaine de caractere du type ZD00400 ou FD00234 */
471  fin_lect_posit = 0;
472  posit_car = 0;
473  lecture_posit_en_cours = 0;
474  do {
475  if (this->read(&c, 1) == true) {
476 
477  // printf("caractere lu: %c\n", c);
478  switch (posit_car) {
479  /* on lit le 1er caractere; (soit Z, soit F, soit D) */
480  case 0:
481  /* sauvegarde du pointeur */
482  pt_posit = posit;
483 
484  switch (servo) {
485  case ZOOM:
486  if (c == 'Z')
487  posit_car = 1;
488  break;
489  case FOCUS:
490  if (c == 'F')
491  posit_car = 1;
492  break;
493  case IRIS:
494  if (c == 'D')
495  posit_car = 1;
496  break;
497  }
498  break;
499 
500  /* si le 1er caractere est correct, on lit le 2eme caractere */
501  /* (toujours D) */
502  case 1:
503  if (c == 'D')
504  posit_car = 2;
505  else
506  posit_car = 0; /* le 2eme caractere n'est pas correct */
507  break;
508 
509  /* si on a lu les 2 premiers caracteres, on peut lire la */
510  /* position du servo-moteur */
511  case 2:
512  if (c >= '0' && c <= '9') {
513  *pt_posit++ = c; /* sauvegarde de la position */
514  lecture_posit_en_cours = 1;
515  } else if (lecture_posit_en_cours) {
516  fin_lect_posit = 1;
517  *pt_posit = '\0';
518  } else
519  posit_car = 0;
520  break;
521  }
522 
523  } else {
524  // Timout sur la lecture, on retoure FALSE
525  return false;
526  }
527  } while (!fin_lect_posit);
528 
529  // printf("\nChaine lue: posit: %s", posit);
530 
531  /* toilettage de la position courantes lue */
532  this->clean(posit, posit);
533 
534  // printf("\nChaine toilettee: posit: %s", posit);
535  position = (unsigned int)atoi(posit);
536 
537  return (true);
538 }
539 
567 {
568  if (!isinit) {
569  vpERROR_TRACE("Cannot dial with Servolens.");
570  throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
571  }
572  vpCameraParameters cam;
573  double pix_size = 7.4e-6; // Specific to the Dragonfly2 camera
574  double px = 1000, py = 1000, u0 = 320, v0 = 240;
575  // Determine if the image is subsampled.
576  // Dragonfly2 native images are 640 by 480
577  double subsample_factor = 1.;
578  double width = I.getWidth();
579  double height = I.getHeight();
580 
581  if (width > 300 && width < 340 && height > 220 && height < 260)
582  subsample_factor = 2;
583  else if (width > 140 && width < 1800 && height > 100 && height < 140)
584  subsample_factor = 4;
585 
586  unsigned zoom;
588  // std::cout << "Actual zoom value: " << zoom << std::endl;
589 
590  // XSIZE_PIX_CAM_AFMA4 / focale et YSIZE_PIX_CAM_AFMA4 / focale
591  // correspondent aux parametres de calibration de la camera (donnees
592  // constructeur) pour des tailles d'images CCIR (768x576), donc avec scale
593  // = 1.
594  double focale = zoom * 1.0e-5; // Transformation en metres
595  px = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
596  py = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
597  u0 = I.getWidth() / 2.;
598  v0 = I.getHeight() / 2.;
599  cam.initPersProjWithoutDistortion(px, py, u0, v0);
600 
601  return cam;
602 }
603 
612 char vpServolens::wait() const
613 {
614  if (!isinit) {
615  vpERROR_TRACE("Cannot dial with Servolens.");
616  throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
617  }
618 
619  ssize_t r;
620  r = ::write(this->remfd, "\r\n", strlen("\r\n"));
621  if (r != (ssize_t)(strlen("\r\n"))) {
622  throw vpRobotException(vpRobotException::communicationError, "Cannot write on Servolens.");
623  }
624  char c;
625  do {
626  r = ::read(this->remfd, &c, 1);
627  c &= 0x7f;
628  if (r != 1) {
629  throw vpRobotException(vpRobotException::communicationError, "Cannot read on Servolens.");
630  }
631  } while (c != '>');
632  return c;
633 }
634 
645 void vpServolens::wait(vpServoType servo) const
646 {
647  if (!isinit) {
648  vpERROR_TRACE("Cannot dial with Servolens.");
649  throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
650  }
651 
652  char c;
653  std::string fin_mvt;
654  bool sortie = false;
655 
656  switch (servo) {
657  case ZOOM:
658  fin_mvt = "ZF";
659  break;
660  case FOCUS:
661  fin_mvt = "FF";
662  break;
663  case IRIS:
664  default:
665  fin_mvt = "DF";
666  break;
667  }
668 
669  /* lecture des caracteres recus */
670  do {
671  /* lecture des caracteres */
672  if (::read(this->remfd, &c, 1) != 1) {
673  throw vpRobotException(vpRobotException::communicationError, "Cannot read on Servolens.");
674  }
675  c &= 0x7f;
676 
677  /* tests si fin de mouvement */
678  if (c == fin_mvt.c_str()[0]) {
679  /* lecture du caractere suivant */
680  if (::read(this->remfd, &c, 1) != 1) {
681  throw vpRobotException(vpRobotException::communicationError, "Cannot read on Servolens.");
682  }
683 
684  c &= 0x7f;
685  if (c == fin_mvt.c_str()[1]) {
686  sortie = true;
687  }
688  }
689  } while (!sortie);
690 
691  /* printf("\nmouvement fini: chaine lue = %s", chaine); */
692 }
693 
705 bool vpServolens::read(char *c, long timeout_s) const
706 {
707  if (!isinit) {
708  vpERROR_TRACE("Cannot dial with Servolens.");
709  throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
710  }
711 
712  fd_set readfds; /* list of fds for select to listen to */
713  struct timeval timeout = {timeout_s, 0}; // seconde, micro-sec
714 
715  FD_ZERO(&readfds);
716  FD_SET(static_cast<unsigned int>(this->remfd), &readfds);
717 
718  if (select(FD_SETSIZE, &readfds, (fd_set *)nullptr, (fd_set *)nullptr, &timeout) > 0) {
719  ssize_t n = ::read(this->remfd, c, 1); /* read one character at a time */
720  if (n != 1)
721  return false;
722  *c &= 0x7f;
723  // printf("lecture 1 car: %c\n", *c);
724  return (true);
725  }
726 
727  return (false);
728 }
729 
738 void vpServolens::write(const char *s) const
739 {
740  if (!isinit) {
741  vpERROR_TRACE("Cannot dial with Servolens.");
742  throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
743  }
744  ssize_t r = 0;
745  r = ::write(this->remfd, "\r", strlen("\r"));
746  r += ::write(this->remfd, s, strlen(s));
747  r += ::write(this->remfd, "\r", strlen("\r"));
748  if (r != (ssize_t)(2 * strlen("\r") + strlen(s))) {
749  throw vpRobotException(vpRobotException::communicationError, "Cannot write on Servolens.");
750  }
751 
752  /*
753  * Une petite tempo pour laisser le temps a la liaison serie de
754  * digerer la commande envoyee. En fait, la liaison serie fonctionne
755  * a 9600 bauds soit une transmission d'environ 9600 bits pas seconde.
756  * Les plus longues commandes envoyees sur la liaison serie sont du type:
757  * SX0842 soit 6 caracteres codes sur 8 bits chacuns = 48 bits pour
758  * envoyer la commande SX0842.
759  * Ainsi, le temps necessaire pour envoyer SX0842 est d'environ
760  * 48 / 9600 = 0,0050 secondes = 5 milli secondes.
761  * Ici on rajoute une marge pour amener la tempo a 20 ms.
762  */
763  vpTime::wait(20);
764 }
765 
773 bool vpServolens::clean(const char *in, char *out) const
774 {
775  short nb_car, i = 0;
776  bool error = false;
777 
778  nb_car = strlen(in);
779 
780  /* on se positionne sur le 1er caractere different de zero */
781  while (*(in) == '0' && i++ < nb_car) {
782  in++;
783  if (i == nb_car) {
784  error = true; /* la chaine ne contient pas une position */
785  *(out++) = '0'; /* mise a zero de la position */
786  }
787  }
788 
789  /* copie de la position epuree des zeros de gauche */
790  while (i++ <= nb_car) { /* on a mis le = pour copier le caractere de fin */
791  /* de chaine \0 */
792  *(out++) = *(in++);
793  }
794  return (error);
795 }
796 
797 #endif
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
unsigned int getWidth() const
Definition: vpImage.h:245
unsigned int getHeight() const
Definition: vpImage.h:184
Error that can be emitted by the vpRobot class and its derivatives.
@ communicationError
Unable to communicate.
void setPosition(vpServoType servo, unsigned int position) const
vpCameraParameters getCameraParameters(vpImage< unsigned char > &I) const
void setController(vpControllerType controller) const
void reset() const
bool getPosition(vpServoType servo, unsigned int &position) const
virtual ~vpServolens()
Definition: vpServolens.cpp:87
void setAutoIris(bool enable) const
void open(const char *port="/dev/ttyS0")
void enablePrompt(bool active) const
#define vpERROR_TRACE
Definition: vpDebug.h:382
VISP_EXPORT int wait(double t0, double t)