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