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