Visual Servoing Platform version 3.5.0
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
71vpServolens::vpServolens() : remfd(0), isinit(false) {}
72
81vpServolens::vpServolens(const char *port) : remfd(0), isinit(false) { this->open(port); }
82
91
106void 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
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}
229void 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
260void 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
323void 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
349void 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/*
359printf("attente prompt\n");
360this->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
434bool 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 }
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
615char 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
648void 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
708bool 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
741void 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
776bool 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
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
Error that can be emited by the vpRobot class and its derivates.
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:90
void setAutoIris(bool enable) const
void open(const char *port="/dev/ttyS0")
void enablePrompt(bool active) const
#define vpERROR_TRACE
Definition: vpDebug.h:393
VISP_EXPORT int wait(double t0, double t)