vpServolens.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Interface for the Servolens lens attached to the camera fixed on the
 * Afma4 robot.
 *
*****************************************************************************/
 
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
 
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <termios.h>
#include <unistd.h>
 
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpTime.h>
#include <visp3/robot/vpRobotException.h>
#include <visp3/robot/vpServolens.h>
 
BEGIN_VISP_NAMESPACE
vpServolens::vpServolens() : remfd(0), isinit(false) { }
 
vpServolens::vpServolens(const char *port) : remfd(0), isinit(false) { this->open(port); }
 
vpServolens::~vpServolens() { this->close(); }
 
void vpServolens::open(const char *port)
{
  if (!isinit) {
    struct termios info;
 
    printf("\nOpen the Servolens serial port \"%s\"\n", port);
 
    if ((this->remfd = ::open(port, O_RDWR | O_NONBLOCK)) < 0) {
      vpERROR_TRACE("Cannot open Servolens serial port.");
      throw vpRobotException(vpRobotException::communicationError, "Cannot open Servolens serial port.");
    }
 
    // Lecture des parametres courants de la liaison serie.
    if (tcgetattr(this->remfd, &info) < 0) {
      ::close(this->remfd);
      vpERROR_TRACE("Error using TCGETS in ioctl.");
      throw vpRobotException(vpRobotException::communicationError, "Error using TCGETS in ioctl");
    }
 
    //
    // Configuration de la liaison serie:
    // 9600 bauds, 1 bit de stop, parite paire, 7 bits de donnee
    //
 
    // Traitement sur les caracteres recus
    info.c_iflag = 0;
    info.c_iflag |= INLCR;
 
    // Traitement sur les caracteres envoyes sur la RS232.
    info.c_oflag = 0; // idem
 
    // Traitement des lignes
    info.c_lflag = 0;
 
    // Controle materiel de la liaison
    info.c_cflag = 0;
    info.c_cflag |= CREAD;                // Validation reception
    info.c_cflag |= B9600 | CS7 | PARENB; // 9600 baus, 7 data, parite paire
 
    // Caracteres immediatement disponibles.
    //  info.c_cc[VMIN] = 1;
    //  info.c_cc[VTIME] = 0;
 
    if (tcsetattr(this->remfd, TCSANOW, &info) < 0) {
      ::close(this->remfd);
      vpERROR_TRACE("Error using TCGETS in ioctl.");
      throw vpRobotException(vpRobotException::communicationError, "Error using TCGETS in ioctl");
    }
 
    // Supprime tous les caracteres recus mais non encore lus par read()
    tcflush(this->remfd, TCIFLUSH);
 
    isinit = true;
 
    this->init();
 
    // Try to get the position of the zoom to check if the lens is really
    // connected
    unsigned int izoom;
    if (this->getPosition(vpServolens::ZOOM, izoom) == false) {
      vpERROR_TRACE("Cannot dial with the servolens. Check if the serial "
                    "link is connected.");
      throw vpRobotException(vpRobotException::communicationError, "Cannot dial with the servolens. Check if the "
                                                                   "serial link is connected.");
    }
  }
}
 
void vpServolens::close()
{
  if (isinit) {
    printf("\nClose the serial connection with Servolens\n");
    ::close(this->remfd);
    isinit = false;
  }
}
 
void vpServolens::reset() const
{
  if (!isinit) {
    vpERROR_TRACE("Cannot dial with Servolens.");
    throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
  }
  std::string cmd;
 
  /* suppression de l'echo */
  cmd = "SE1";
  this->write(cmd.c_str());
 
  /* initialisation de l'objectif, idem qu'a la mise sous tension */
  cmd = "SR0";
  this->write(cmd.c_str());
 
  vpTime::wait(25000);
 
  this->wait();
 
  /* suppression de l'echo */
  cmd = "SE0";
  this->write(cmd.c_str());
 
  /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
  cmd = "VW0";
  this->write(cmd.c_str());
}
void vpServolens::init() const
{
  if (!isinit) {
    vpERROR_TRACE("Cannot dial with Servolens.");
    throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
  }
 
  std::string cmd;
 
  /* suppression de l'echo */
  cmd = "SE0";
  this->write(cmd.c_str());
 
  /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
  cmd = "VW0";
  this->write(cmd.c_str());
 
  /* L'experience montre qu'une petite tempo est utile. */
  vpTime::wait(500);
}
 
void vpServolens::enablePrompt(bool active) const
{
  if (!isinit) {
    vpERROR_TRACE("Cannot dial with Servolens.");
    throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
  }
  std::string cmd;
 
  /* suppression de l'echo */
  if (active == true)
    cmd = "SE1";
  else
    cmd = "SE0";
 
  this->write(cmd.c_str());
}
 
void vpServolens::setController(vpControllerType controller) const
{
  if (!isinit) {
    vpERROR_TRACE("Cannot dial with Servolens.");
    throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
  }
  std::string cmd;
 
  switch (controller) {
  case AUTO:
    /* Valide l'incrustation de la fenetre sur l'ecran du moniteur */
    cmd = "VW1";
    this->write(cmd.c_str());
    break;
  case CONTROLLED:
    /* nettoyage : mot d'etat vide 0000 */
    cmd = "SX0842";
    this->write(cmd.c_str());
    /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
    cmd = "VW0";
    this->write(cmd.c_str());
    break;
  case RELEASED:
    cmd = "SX1084";
    this->write(cmd.c_str());
    /* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
    cmd = "VW0";
    this->write(cmd.c_str());
    break;
  }
}
 
void vpServolens::setAutoIris(bool enable) const
{
  if (!isinit) {
    vpERROR_TRACE("Cannot dial with Servolens.");
    throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
  }
  std::string cmd;
 
  if (enable)
    cmd = "DA1";
  else
    cmd = "DA0";
 
  this->write(cmd.c_str());
}
 
void vpServolens::setPosition(vpServoType servo, unsigned int position) const
{
  if (!isinit) {
    vpERROR_TRACE("Cannot dial with Servolens.");
    throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
  }
  std::stringstream command;
 
  /* attente du prompt pour envoyer une commande */
  /*
  printf("attente prompt\n");
  this->wait();
  */
 
#ifdef FINSERVO
  /* envoie des commandes pour qu'en fin de mouvement servolens renvoie */
  /* une commande de fin de mouvement (ex: ZF, FF, DF). */
  this->enableCommandComplete();
#endif /* FINSERVO */
 
  // 08/08/00 Fabien S. - Correction de la consigne demandee
  // pour prendre en compte l'erreur entre la consigne demandee
  // et la consigne mesuree.
  // A la consigne du zoom on retranche 1.
  // A la consigne du focus on ajoute 1.
  // A la consigne du iris on ajoute 1.
  switch (servo) {
  case ZOOM:
    // printf("zoom demande: %d ", position);
    position--;
    if (position < ZOOM_MIN)
      position = ZOOM_MIN;
    // printf("zoom corrige: %d \n", position);
    break;
  case FOCUS:
    // printf("focus demande: %d ", position);
    position++;
    if (position > FOCUS_MAX)
      position = FOCUS_MAX;
    // printf("focus corrige: %d \n", position);
    break;
  case IRIS:
    // printf("iris demande: %d ", position);
    position++;
    if (position > IRIS_MAX)
      position = IRIS_MAX;
    // printf("iris corrige: %s \n", position);
    break;
  }
 
  /* commande a envoyer aux servomoteurs */
  switch (servo) {
  case ZOOM:
    command << "ZD" << position;
    break;
  case FOCUS:
    command << "FD" << position;
    break;
  case IRIS:
    command << "DD" << position;
    break;
  }
/* envoie de la commande */
#ifdef PRINT
  printf("\ncommand: %s", command.str());
#endif
 
  this->write(command.str().c_str());
 
#ifdef FINSERVO
  /* on attend la fin du mouvement des objectifs */
  this->wait(servo); /* on attend les codes ZF, FF, DF */
#endif
}
 
bool vpServolens::getPosition(vpServoType servo, unsigned int &position) const
{
  if (!isinit) {
    vpERROR_TRACE("Cannot dial with Servolens.");
    throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
  }
  std::string cmd;
  char posit[10], *pt_posit;
  char c;
  short fin_lect_posit;         /* indique si on a lu la position du servo-moteur */
  short posit_car;              /* donne la position du caractere lu */
  short lecture_posit_en_cours; /* indique si on lit la position courante */
 
  /* attente du prompt pour envoyer une commande */
  /*
  this->wait();
  */
  pt_posit = posit;
 
  /* envoie des commandes pour obtenir la position des servo-moteurs. */
  switch (servo) {
  case ZOOM:
    cmd = "ZD?";
    break;
  case FOCUS:
    cmd = "FD?";
    break;
  case IRIS:
    cmd = "DD?";
    break;
  default:
    break;
  }
  /* envoie de la commande */
  //    printf("\ncommande: %s", commande);
 
  this->write(cmd.c_str());
 
  /* on cherche a lire la position du servo-moteur */
  /* Servolens renvoie une chaine de caractere du type ZD00400 ou FD00234 */
  fin_lect_posit = 0;
  posit_car = 0;
  lecture_posit_en_cours = 0;
  do {
    if (this->read(&c, 1) == true) {
 
      //    printf("caractere lu: %c\n", c);
      switch (posit_car) {
      /* on lit le 1er caractere; (soit Z, soit F, soit D) */
      case 0:
        /* sauvegarde du pointeur */
        pt_posit = posit;
 
        switch (servo) {
        case ZOOM:
          if (c == 'Z')
            posit_car = 1;
          break;
        case FOCUS:
          if (c == 'F')
            posit_car = 1;
          break;
        case IRIS:
          if (c == 'D')
            posit_car = 1;
          break;
        }
        break;
 
      /* si le 1er caractere est correct, on lit le 2eme caractere */
      /* (toujours D) */
      case 1:
        if (c == 'D')
          posit_car = 2;
        else
          posit_car = 0; /* le 2eme caractere n'est pas correct */
        break;
 
      /* si on a lu les 2 premiers caracteres, on peut lire la */
      /* position du servo-moteur */
      case 2:
        if (c >= '0' && c <= '9') {
          *pt_posit++ = c; /* sauvegarde de la position */
          lecture_posit_en_cours = 1;
        }
        else if (lecture_posit_en_cours) {
          fin_lect_posit = 1;
          *pt_posit = '\0';
        }
        else
          posit_car = 0;
        break;
      }
 
    }
    else {
   // Timout sur la lecture, on retoure FALSE
      return false;
    }
  } while (!fin_lect_posit);
 
  //    printf("\nChaine lue: posit: %s", posit);
 
  /* toilettage de la position courantes lue */
  this->clean(posit, posit);
 
  //    printf("\nChaine toilettee: posit: %s", posit);
  position = (unsigned int)atoi(posit);
 
  return (true);
}
 
vpCameraParameters vpServolens::getCameraParameters(vpImage<unsigned char> &I) const
{
  if (!isinit) {
    vpERROR_TRACE("Cannot dial with Servolens.");
    throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
  }
  vpCameraParameters cam;
  double pix_size = 7.4e-6; // Specific to the Dragonfly2 camera
  double px = 1000, py = 1000, u0 = 320, v0 = 240;
  // Determine if the image is subsampled.
  // Dragonfly2 native images are 640 by 480
  double subsample_factor = 1.;
  double width = I.getWidth();
  double height = I.getHeight();
 
  if (width > 300 && width < 340 && height > 220 && height < 260)
    subsample_factor = 2;
  else if (width > 140 && width < 1800 && height > 100 && height < 140)
    subsample_factor = 4;
 
  unsigned zoom;
  getPosition(vpServolens::ZOOM, zoom);
  // std::cout << "Actual zoom value: " << zoom << std::endl;
 
  // XSIZE_PIX_CAM_AFMA4 / focale et YSIZE_PIX_CAM_AFMA4 / focale
  // correspondent aux parametres de calibration de la camera (donnees
  // constructeur) pour des tailles d'images CCIR (768x576), donc avec scale
  // = 1.
  double focale = zoom * 1.0e-5;                       // Transformation en metres
  px = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
  py = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
  u0 = I.getWidth() / 2.;
  v0 = I.getHeight() / 2.;
  cam.initPersProjWithoutDistortion(px, py, u0, v0);
 
  return cam;
}
 
char vpServolens::wait() const
{
  if (!isinit) {
    vpERROR_TRACE("Cannot dial with Servolens.");
    throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
  }
 
  ssize_t r;
  r = ::write(this->remfd, "\r\n", strlen("\r\n"));
  if (r != (ssize_t)(strlen("\r\n"))) {
    throw vpRobotException(vpRobotException::communicationError, "Cannot write on Servolens.");
  }
  char c;
  do {
    r = ::read(this->remfd, &c, 1);
    c &= 0x7f;
    if (r != 1) {
      throw vpRobotException(vpRobotException::communicationError, "Cannot read on Servolens.");
    }
  } while (c != '>');
  return c;
}
 
void vpServolens::wait(vpServoType servo) const
{
  if (!isinit) {
    vpERROR_TRACE("Cannot dial with Servolens.");
    throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
  }
 
  char c;
  std::string fin_mvt;
  bool sortie = false;
 
  switch (servo) {
  case ZOOM:
    fin_mvt = "ZF";
    break;
  case FOCUS:
    fin_mvt = "FF";
    break;
  case IRIS:
  default:
    fin_mvt = "DF";
    break;
  }
 
  /* lecture des caracteres recus */
  do {
    /* lecture des caracteres */
    if (::read(this->remfd, &c, 1) != 1) {
      throw vpRobotException(vpRobotException::communicationError, "Cannot read on Servolens.");
    }
    c &= 0x7f;
 
    /* tests si fin de mouvement */
    if (c == fin_mvt.c_str()[0]) {
      /* lecture du caractere suivant */
      if (::read(this->remfd, &c, 1) != 1) {
        throw vpRobotException(vpRobotException::communicationError, "Cannot read on Servolens.");
      }
 
      c &= 0x7f;
      if (c == fin_mvt.c_str()[1]) {
        sortie = true;
      }
    }
  } while (!sortie);
 
  /*  printf("\nmouvement fini: chaine lue = %s", chaine); */
}
 
bool vpServolens::read(char *c, long timeout_s) const
{
  if (!isinit) {
    vpERROR_TRACE("Cannot dial with Servolens.");
    throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
  }
 
  fd_set readfds;                          /* list of fds for select to listen to */
  struct timeval timeout = { timeout_s, 0 }; // seconde, micro-sec
 
  FD_ZERO(&readfds);
  FD_SET(static_cast<unsigned int>(this->remfd), &readfds);
 
  if (select(FD_SETSIZE, &readfds, (fd_set *)nullptr, (fd_set *)nullptr, &timeout) > 0) {
    ssize_t n = ::read(this->remfd, c, 1); /* read one character at a time */
    if (n != 1)
      return false;
    *c &= 0x7f;
    // printf("lecture 1 car: %c\n", *c);
    return (true);
  }
 
  return (false);
}
 
void vpServolens::write(const char *s) const
{
  if (!isinit) {
    vpERROR_TRACE("Cannot dial with Servolens.");
    throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
  }
  ssize_t r = 0;
  r = ::write(this->remfd, "\r", strlen("\r"));
  r += ::write(this->remfd, s, strlen(s));
  r += ::write(this->remfd, "\r", strlen("\r"));
  if (r != (ssize_t)(2 * strlen("\r") + strlen(s))) {
    throw vpRobotException(vpRobotException::communicationError, "Cannot write on Servolens.");
  }
 
  /*
   * Une petite tempo pour laisser le temps a la liaison serie de
   * digerer la commande envoyee. En fait, la liaison serie fonctionne
   * a 9600 bauds soit une transmission d'environ 9600 bits pas seconde.
   * Les plus longues commandes envoyees sur la liaison serie sont du type:
   * SX0842 soit 6 caracteres codes sur 8 bits chacuns = 48 bits pour
   * envoyer la commande SX0842.
   * Ainsi, le temps necessaire pour envoyer SX0842 est d'environ
   * 48 / 9600 = 0,0050 secondes = 5 milli secondes.
   * Ici on rajoute une marge pour amener la tempo a 20 ms.
   */
  vpTime::wait(20);
}
 
bool vpServolens::clean(const char *in, char *out) const
{
  short nb_car, i = 0;
  bool error = false;
 
  nb_car = strlen(in);
 
  /* on se positionne sur le 1er caractere different de zero */
  while (*(in) == '0' && i++ < nb_car) {
    in++;
    if (i == nb_car) {
      error = true;   /* la chaine ne contient pas une position */
      *(out++) = '0'; /* mise a zero de la position */
    }
  }
 
  /* copie de la position epuree des zeros de gauche */
  while (i++ <= nb_car) { /* on a mis le = pour copier le caractere de fin */
    /* de chaine \0 */
    *(out++) = *(in++);
  }
  return (error);
}
END_VISP_NAMESPACE
#endif