vpRobotWireFrameSimulator.h

/****************************************************************************
 *
 * $Id: vpRobotWireFrameSimulator.h 3530 2012-01-03 10:52:12Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 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
 * ("GPL") version 2 as published by the Free Software Foundation.
 * 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://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * 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 <visp/vpConfig.h>



#if defined(_WIN32) || defined(VISP_HAVE_PTHREAD)
#include <cmath>    // std::fabs
#include <limits>   // numeric_limits
#if defined(_WIN32)
#  include <windows.h>
#elif defined(VISP_HAVE_PTHREAD)
#  include <pthread.h>
#endif

#include <visp/vpWireFrameSimulator.h>
#include <visp/vpRobot.h>
#include <visp/vpRobotSimulator.h>
#include <visp/vpDisplayOpenCV.h>
#include <visp/vpDisplayX.h>
#include <visp/vpDisplayGTK.h>
#include <visp/vpDisplayGDI.h>
#include <visp/vpDisplayD3D.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_;
    
  public:
    vpRobotWireFrameSimulator();
    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::minTimeForUsleepCall * 1e-3)){
        this->delta_t_ = static_cast<float>(vpTime::minTimeForUsleepCall * 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 ) 
    {
    ((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 {
      WaitForSingleObject(mutex_artCoord,INFINITE);
      vpColVector artCoordTmp (6);
      artCoordTmp = artCoord;
      ReleaseMutex(mutex_artCoord);
      return artCoordTmp;}
    void set_artCoord(const vpColVector &coord) {
      WaitForSingleObject(mutex_artCoord,INFINITE);
      artCoord = coord;
      ReleaseMutex(mutex_artCoord);}
    
    vpColVector get_artVel() const {
      WaitForSingleObject(mutex_artVel,INFINITE);
      vpColVector artVelTmp (artVel);
      ReleaseMutex(mutex_artVel);
      return artVelTmp;}
    void set_artVel(const vpColVector &vel) {
      WaitForSingleObject(mutex_artVel,INFINITE);
      artVel = vel;
      ReleaseMutex(mutex_artVel);}
    
    vpColVector get_velocity() {
      WaitForSingleObject(mutex_velocity,INFINITE);
      vpColVector velocityTmp = velocity;
      ReleaseMutex(mutex_velocity);
      return velocityTmp;}
    void set_velocity(const vpColVector &vel) {
      WaitForSingleObject(mutex_velocity,INFINITE);
      velocity = vel;
      ReleaseMutex(mutex_velocity);}
      
    void set_displayBusy (const bool &status) {
      WaitForSingleObject(mutex_display,INFINITE);
      displayBusy = status;
      ReleaseMutex(mutex_display);}
    bool get_displayBusy () {
      WaitForSingleObject(mutex_display,INFINITE);
      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