vpRobotWireFrameSimulator.h

/*
 * 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:
 * Basic class used to make robot simulators.
 */
 
#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/core/vpMutex.h>
#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;
#elif defined(VISP_HAVE_PTHREAD)
  pthread_t thread;
  pthread_attr_t attr;
#endif
 
  vpMutex m_mutex_fMi;
  vpMutex m_mutex_eMc;
  vpMutex m_mutex_artVel;
  vpMutex m_mutex_artCoord;
  vpMutex m_mutex_velocity;
  vpMutex m_mutex_display;
  vpMutex m_mutex_robotStop;
  vpMutex m_mutex_frame;
  vpMutex m_mutex_setVelocityCalled;
  vpMutex m_mutex_scene;
 
  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(HAVE_OPENCV_HIGHGUI)
  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_;
 
public:
  vpRobotWireFrameSimulator();
  explicit vpRobotWireFrameSimulator(bool display);
 
  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(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 nullptr;
  }
#endif
 
  virtual void updateArticularPosition() = 0;
  virtual int isInJointLimit() = 0;
  virtual void computeArticularVelocity() = 0;
 
  /* Display functions */
  void initDisplay() { ; }
  virtual void initArms() = 0;
 
  vpColVector get_artCoord()
  {
    m_mutex_artCoord.lock();
    vpColVector artCoordTmp(6);
    artCoordTmp = artCoord;
    m_mutex_artCoord.unlock();
    return artCoordTmp;
  }
  void set_artCoord(const vpColVector &coord)
  {
    m_mutex_artCoord.lock();
    artCoord = coord;
    m_mutex_artCoord.unlock();
  }
 
  vpColVector get_artVel()
  {
    m_mutex_artVel.lock();
    vpColVector artVelTmp(artVel);
    m_mutex_artVel.unlock();
    return artVelTmp;
  }
  void set_artVel(const vpColVector &vel)
  {
    m_mutex_artVel.lock();
    artVel = vel;
    m_mutex_artVel.unlock();
  }
 
  vpColVector get_velocity()
  {
    m_mutex_velocity.lock();
    vpColVector velocityTmp = velocity;
    m_mutex_velocity.unlock();
    return velocityTmp;
  }
  void set_velocity(const vpColVector &vel)
  {
    m_mutex_velocity.lock();
    velocity = vel;
    m_mutex_velocity.unlock();
  }
 
  void set_displayBusy(const bool &status)
  {
    m_mutex_display.lock();
    displayBusy = status;
    m_mutex_display.unlock();
  }
  bool get_displayBusy()
  {
    m_mutex_display.lock();
    bool status = displayBusy;
    if (!displayBusy)
      displayBusy = true;
    m_mutex_display.unlock();
    return status;
  }
 
  virtual void get_fMi(vpHomogeneousMatrix *fMit) = 0;
};
 
#endif
#endif