vpRobotWireFrameSimulator.h

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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 http://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:
 * Basic class used to make robot simulators.
 *
 * Authors:
 * Nicolas Melchior
 *
 *****************************************************************************/

#ifndef vpRobotWireFrameSimulator_HH
#define vpRobotWireFrameSimulator_HH

#include <visp3/core/vpConfig.h>

#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))

#include <cmath>  // std::fabs
#include <limits> // numeric_limits
#if defined(_WIN32)
// Include WinSock2.h before windows.h to ensure that winsock.h is not
// included by windows.h since winsock.h and winsock2.h are incompatible
#include <WinSock2.h>
#include <windows.h>
#elif defined(VISP_HAVE_PTHREAD)
#include <pthread.h>
#endif

#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobot.h>
#include <visp3/robot/vpRobotSimulator.h>
#include <visp3/robot/vpWireFrameSimulator.h>

class VISP_EXPORT vpRobotWireFrameSimulator : protected vpWireFrameSimulator, public vpRobotSimulator
{
public:
  vpImage<vpRGBa> I;

  typedef enum { MODEL_3D, MODEL_DH } vpDisplayRobotType;

protected:
  double tcur;
  double tprev;

  Bound_scene *robotArms;

  unsigned int size_fMi;
  vpHomogeneousMatrix *fMi;

  vpColVector artCoord;
  vpColVector artVel;
  vpColVector velocity;

#if defined(_WIN32)
  HANDLE hThread;
  HANDLE mutex_fMi;
  HANDLE mutex_artVel;
  HANDLE mutex_artCoord;
  HANDLE mutex_velocity;
  HANDLE mutex_display;
#elif defined(VISP_HAVE_PTHREAD)
  pthread_t thread;
  pthread_attr_t attr;
  pthread_mutex_t mutex_fMi;
  pthread_mutex_t mutex_artVel;
  pthread_mutex_t mutex_artCoord;
  pthread_mutex_t mutex_velocity;
  pthread_mutex_t mutex_display;
#endif

  bool displayBusy;

  bool robotStop;
  bool jointLimit;
  unsigned int jointLimitArt;
  bool singularityManagement;

  vpCameraParameters cameraParam;

#if defined VISP_HAVE_X11
  vpDisplayX display;
#elif defined VISP_HAVE_GDI
  vpDisplayGDI display;
#elif defined VISP_HAVE_OPENCV
  vpDisplayOpenCV display;
#elif defined VISP_HAVE_D3D9
  vpDisplayD3D display;
#elif defined VISP_HAVE_GTK
  vpDisplayGTK display;
#endif

  vpDisplayRobotType displayType;

  bool displayAllowed;
  bool constantSamplingTimeMode;

  bool setVelocityCalled;

  bool verbose_;

  // private:
  //#ifndef DOXYGEN_SHOULD_SKIP_THIS
  //    vpRobotWireFrameSimulator(const vpRobotWireFrameSimulator &)
  //      : vpWireFrameSimulator(), vpRobotSimulator(),
  //        I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL),
  //        artCoord(), artVel(), velocity(),
  //    #if defined(_WIN32)
  //    #elif defined(VISP_HAVE_PTHREAD)
  //        thread(), attr(),
  //    #endif
  //        mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(),
  //        mutex_display(), displayBusy(false), robotStop(false),
  //        jointLimit(false), jointLimitArt(false),
  //        singularityManagement(true), cameraParam(),
  //    #if defined(VISP_HAVE_DISPLAY)
  //        display(),
  //    #endif
  //        displayType(MODEL_3D), displayAllowed(true),
  //        constantSamplingTimeMode(false), setVelocityCalled(false),
  //        verbose_(false)
  //    {
  //      throw vpException(vpException::functionNotImplementedError, "Not
  //      implemented!");
  //    }
  //    vpRobotWireFrameSimulator &operator=(const vpRobotWireFrameSimulator
  //    &){
  //      throw vpException(vpException::functionNotImplementedError, "Not
  //      implemented!"); return *this;
  //    }
  //#endif

public:
  vpRobotWireFrameSimulator();
  explicit vpRobotWireFrameSimulator(bool display);
  virtual ~vpRobotWireFrameSimulator();

  vpCameraParameters getExternalCameraParameters() const
  {
    // if(px_ext != 1 && py_ext != 1)
    // we assume px_ext and py_ext > 0
    if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
        (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
      return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
    else {
      unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
      return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
    }
  }
  vpHomogeneousMatrix getExternalCameraPosition() const
  {
    return this->vpWireFrameSimulator::getExternalCameraPosition();
  }

  void getInternalView(vpImage<vpRGBa> &I);
  void getInternalView(vpImage<unsigned char> &I);

  vpHomogeneousMatrix get_cMo();
  vpHomogeneousMatrix get_fMo() const { return fMo; }

  /* Display functions */
  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
  void initScene(const char *obj, const char *desiredObject);
  void initScene(const vpSceneObject &obj);
  void initScene(const char *obj);

  void setCameraColor(const vpColor &col) { camColor = col; }

  inline void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode)
  {
    constantSamplingTimeMode = _constantSamplingTimeMode;
  }

  void setCurrentViewColor(const vpColor &col) { curColor = col; }

  void setDesiredViewColor(const vpColor &col) { desColor = col; }

  void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
  {
    this->vpWireFrameSimulator::setDesiredCameraPosition(cdMo_);
  }

  inline void setDisplayRobotType(const vpDisplayRobotType dispType) { displayType = dispType; }
  void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
  {
    this->vpWireFrameSimulator::setExternalCameraPosition(camMf_);
  }
  void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; }

  inline void setSamplingTime(const double &delta_t)
  {
    if (delta_t < static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3)) {
      this->delta_t_ = static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3);
    } else {
      this->delta_t_ = delta_t;
    }
  }
  void setSingularityManagement(const bool sm) { singularityManagement = sm; }

  void setVerbose(bool verbose) { this->verbose_ = verbose; }

  void set_fMo(const vpHomogeneousMatrix &fMo_) { this->fMo = fMo_; }

protected:
#if defined(_WIN32)
  static DWORD WINAPI launcher(LPVOID lpParam)
  {
    (static_cast<vpRobotWireFrameSimulator *>(lpParam))->updateArticularPosition();
    return 0;
  }
#elif defined(VISP_HAVE_PTHREAD)
  static void *launcher(void *arg)
  {
    (reinterpret_cast<vpRobotWireFrameSimulator *>(arg))->updateArticularPosition();
    // pthread_exit((void*) 0);
    return NULL;
  }
#endif

  /* Robot functions */
  void init() { ; }
  virtual void updateArticularPosition() = 0;
  virtual int isInJointLimit() = 0;
  virtual void computeArticularVelocity() = 0;

  /* Display functions */
  void initDisplay() { ; }
  virtual void initArms() = 0;

#if defined(_WIN32)
  vpColVector get_artCoord() const
  {
#if defined(WINRT_8_1)
    WaitForSingleObjectEx(mutex_artCoord, INFINITE, FALSE);
#else // pure win32
    WaitForSingleObject(mutex_artCoord, INFINITE);
#endif
    vpColVector artCoordTmp(6);
    artCoordTmp = artCoord;
    ReleaseMutex(mutex_artCoord);
    return artCoordTmp;
  }
  void set_artCoord(const vpColVector &coord)
  {
#if defined(WINRT_8_1)
    WaitForSingleObjectEx(mutex_artCoord, INFINITE, FALSE);
#else // pure win32
    WaitForSingleObject(mutex_artCoord, INFINITE);
#endif
    artCoord = coord;
    ReleaseMutex(mutex_artCoord);
  }

  vpColVector get_artVel() const
  {
#if defined(WINRT_8_1)
    WaitForSingleObjectEx(mutex_artVel, INFINITE, FALSE);
#else // pure win32
    WaitForSingleObject(mutex_artVel, INFINITE);
#endif
    vpColVector artVelTmp(artVel);
    ReleaseMutex(mutex_artVel);
    return artVelTmp;
  }
  void set_artVel(const vpColVector &vel)
  {
#if defined(WINRT_8_1)
    WaitForSingleObjectEx(mutex_artVel, INFINITE, FALSE);
#else // pure win32
    WaitForSingleObject(mutex_artVel, INFINITE);
#endif
    artVel = vel;
    ReleaseMutex(mutex_artVel);
  }

  vpColVector get_velocity()
  {
#if defined(WINRT_8_1)
    WaitForSingleObjectEx(mutex_velocity, INFINITE, FALSE);
#else // pure win32
    WaitForSingleObject(mutex_velocity, INFINITE);
#endif
    vpColVector velocityTmp = velocity;
    ReleaseMutex(mutex_velocity);
    return velocityTmp;
  }
  void set_velocity(const vpColVector &vel)
  {
#if defined(WINRT_8_1)
    WaitForSingleObjectEx(mutex_velocity, INFINITE, FALSE);
#else // pure win32
    WaitForSingleObject(mutex_velocity, INFINITE);
#endif
    velocity = vel;
    ReleaseMutex(mutex_velocity);
  }

  void set_displayBusy(const bool &status)
  {
#if defined(WINRT_8_1)
    WaitForSingleObjectEx(mutex_display, INFINITE, FALSE);
#else // pure win32
    WaitForSingleObject(mutex_display, INFINITE);
#endif
    displayBusy = status;
    ReleaseMutex(mutex_display);
  }
  bool get_displayBusy()
  {
#if defined(WINRT_8_1)
    WaitForSingleObjectEx(mutex_display, INFINITE, FALSE);
#else // pure win32
    WaitForSingleObject(mutex_display, INFINITE);
#endif
    bool status = displayBusy;
    if (!displayBusy)
      displayBusy = true;
    ReleaseMutex(mutex_display);
    return status;
  }

#elif defined(VISP_HAVE_PTHREAD)
  vpColVector get_artCoord()
  {
    pthread_mutex_lock(&mutex_artCoord);
    vpColVector artCoordTmp(6);
    artCoordTmp = artCoord;
    pthread_mutex_unlock(&mutex_artCoord);
    return artCoordTmp;
  }
  void set_artCoord(const vpColVector &coord)
  {
    pthread_mutex_lock(&mutex_artCoord);
    artCoord = coord;
    pthread_mutex_unlock(&mutex_artCoord);
  }

  vpColVector get_artVel()
  {
    pthread_mutex_lock(&mutex_artVel);
    vpColVector artVelTmp(artVel);
    pthread_mutex_unlock(&mutex_artVel);
    return artVelTmp;
  }
  void set_artVel(const vpColVector &vel)
  {
    pthread_mutex_lock(&mutex_artVel);
    artVel = vel;
    pthread_mutex_unlock(&mutex_artVel);
  }

  vpColVector get_velocity()
  {
    pthread_mutex_lock(&mutex_velocity);
    vpColVector velocityTmp = velocity;
    pthread_mutex_unlock(&mutex_velocity);
    return velocityTmp;
  }
  void set_velocity(const vpColVector &vel)
  {
    pthread_mutex_lock(&mutex_velocity);
    velocity = vel;
    pthread_mutex_unlock(&mutex_velocity);
  }

  void set_displayBusy(const bool &status)
  {
    pthread_mutex_lock(&mutex_display);
    displayBusy = status;
    pthread_mutex_unlock(&mutex_display);
  }
  bool get_displayBusy()
  {
    pthread_mutex_lock(&mutex_display);
    bool status = displayBusy;
    if (!displayBusy)
      displayBusy = true;
    pthread_mutex_unlock(&mutex_display);
    return status;
  }
#endif

  virtual void get_fMi(vpHomogeneousMatrix *fMit) = 0;
};

#endif
#endif