ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
vpRobotWireFrameSimulator.h
1 /****************************************************************************
2  *
3  * $Id: vpRobotWireFrameSimulator.h 3530 2012-01-03 10:52:12Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Basic class used to make robot simulators.
36  *
37  * Authors:
38  * Nicolas Melchior
39  *
40  *****************************************************************************/
41 
42 #ifndef vpRobotWireFrameSimulator_HH
43 #define vpRobotWireFrameSimulator_HH
44 
50 #include <visp/vpConfig.h>
51 
52 
53 
54 #if defined(WIN32) || defined(VISP_HAVE_PTHREAD)
55 #include <cmath> // std::fabs
56 #include <limits> // numeric_limits
57 #if defined(WIN32)
58 # include <windows.h>
59 #elif defined(VISP_HAVE_PTHREAD)
60 # include <pthread.h>
61 #endif
62 
63 #include <visp/vpWireFrameSimulator.h>
64 #include <visp/vpRobot.h>
65 #include <visp/vpRobotSimulator.h>
66 #include <visp/vpDisplayOpenCV.h>
67 #include <visp/vpDisplayX.h>
68 #include <visp/vpDisplayGTK.h>
69 #include <visp/vpDisplayGDI.h>
70 #include <visp/vpDisplayD3D.h>
71 
90 {
91  public:
93 
94  typedef enum
95  {
97  MODEL_DH
98  } vpDisplayRobotType;
99 
100 
101  protected:
103  double tcur;
105  double tprev;
106 
108  Bound_scene* robotArms;
109 
111  unsigned int size_fMi;
116 
123 
124  #if defined(WIN32)
125  HANDLE hThread;
126  HANDLE mutex_fMi;
127  HANDLE mutex_artVel;
131  #elif defined(VISP_HAVE_PTHREAD)
132  pthread_t thread;
133  pthread_attr_t attr;
134  pthread_mutex_t mutex_fMi;
135  pthread_mutex_t mutex_artVel;
136  pthread_mutex_t mutex_artCoord;
137  pthread_mutex_t mutex_velocity;
138  pthread_mutex_t mutex_display;
139  #endif
140 
142 
144  bool robotStop;
148  unsigned int jointLimitArt;
151 
154 
155  #if defined VISP_HAVE_X11
157  #elif defined VISP_HAVE_GDI
158  vpDisplayGDI display;
159  #elif defined VISP_HAVE_OPENCV
160  vpDisplayOpenCV display;
161  #elif defined VISP_HAVE_D3D9
162  vpDisplayD3D display;
163  #elif defined VISP_HAVE_GTK
164  vpDisplayGTK display;
165  #endif
166 
168 
172 
175 
176  public:
178  vpRobotWireFrameSimulator(bool display);
179  virtual ~vpRobotWireFrameSimulator();
180 
187  //if(px_ext != 1 && py_ext != 1)
188  // we assume px_ext and py_ext > 0
189  if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
190  && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
191  return vpCameraParameters(px_ext,py_ext,I.getWidth()/2,I.getHeight()/2);
192  else
193  {
194  unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
195  return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
196  }
197  }
204  {
206  }
207 
208  void getInternalView(vpImage<vpRGBa> &I);
209  void getInternalView(vpImage<unsigned char> &I);
210 
217  vpHomogeneousMatrix get_fMo() const {return fMo;}
218 
219  /* Display functions */
220  void initScene(vpSceneObject obj, vpSceneDesiredObject desiredObject);
221  void initScene(const char* obj, const char* desiredObject);
222  void initScene (vpSceneObject obj);
223  void initScene(const char* obj);
224 
230  void setCameraColor(const vpColor col) {camColor = col;}
231 
240  inline void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode){
241  constantSamplingTimeMode = _constantSamplingTimeMode;
242  }
243 
249  void setCurrentViewColor(const vpColor col) {curColor = col;}
250 
256  void setDesiredViewColor(const vpColor col) {desColor = col;}
257 
264  {
266  }
267 
273  inline void setDisplayRobotType (const vpDisplayRobotType dispType) {displayType = dispType;}
280  {
282  }
283 
293  inline void setSamplingTime(const double &delta_t)
294  {
295  if(delta_t < static_cast<float>(vpTime::minTimeForUsleepCall * 1e-3)){
296  this->delta_t_ = static_cast<float>(vpTime::minTimeForUsleepCall * 1e-3);
297  } else {
298  this->delta_t_ = delta_t;
299  }
300  }
302  void setSingularityManagement (const bool sm) {singularityManagement = sm;}
303 
309  void set_fMo(const vpHomogeneousMatrix &fMo) {this->fMo = fMo;}
310 
311  protected:
315  #if defined(WIN32)
316  static DWORD WINAPI launcher( LPVOID lpParam )
317  {
318  ((vpRobotWireFrameSimulator *)lpParam)->updateArticularPosition();
319  return 0;
320  }
321  #elif defined(VISP_HAVE_PTHREAD)
322  static void* launcher(void *arg)
323  {
324  (reinterpret_cast<vpRobotWireFrameSimulator *>(arg))->updateArticularPosition();
325  // pthread_exit((void*) 0);
326  return NULL;
327  }
328  #endif
329 
330  /* Robot functions */
331  void init() {;}
333  virtual void updateArticularPosition() = 0;
335  virtual int isInJointLimit () = 0;
337  virtual void computeArticularVelocity() = 0;
338 
339  /* Display functions */
340  void initDisplay() {;}
341  virtual void initArms() = 0;
342 
343  #if defined(WIN32)
345  WaitForSingleObject(mutex_artCoord,INFINITE);
346  vpColVector artCoordTmp (6);
347  artCoordTmp = artCoord;
348  ReleaseMutex(mutex_artCoord);
349  return artCoordTmp;}
350  void set_artCoord(const vpColVector &coord) {
351  WaitForSingleObject(mutex_artCoord,INFINITE);
352  artCoord = coord;
353  ReleaseMutex(mutex_artCoord);}
354 
356  WaitForSingleObject(mutex_artVel,INFINITE);
357  vpColVector artVelTmp (artVel);
358  ReleaseMutex(mutex_artVel);
359  return artVelTmp;}
360  void set_artVel(const vpColVector &vel) {
361  WaitForSingleObject(mutex_artVel,INFINITE);
362  artVel = vel;
363  ReleaseMutex(mutex_artVel);}
364 
366  WaitForSingleObject(mutex_velocity,INFINITE);
367  vpColVector velocityTmp = velocity;
368  ReleaseMutex(mutex_velocity);
369  return velocityTmp;}
370  void set_velocity(const vpColVector &vel) {
371  WaitForSingleObject(mutex_velocity,INFINITE);
372  velocity = vel;
373  ReleaseMutex(mutex_velocity);}
374 
375  void set_displayBusy (const bool &status) {
376  WaitForSingleObject(mutex_display,INFINITE);
377  displayBusy = status;
378  ReleaseMutex(mutex_display);}
379  bool get_displayBusy () {
380  WaitForSingleObject(mutex_display,INFINITE);
381  bool status = displayBusy;
382  if (!displayBusy) displayBusy = true;
383  ReleaseMutex(mutex_display);
384  return status;}
385 
386  #elif defined(VISP_HAVE_PTHREAD)
387  vpColVector get_artCoord() {
388  pthread_mutex_lock (&mutex_artCoord);
389  vpColVector artCoordTmp (6);
390  artCoordTmp = artCoord;
391  pthread_mutex_unlock (&mutex_artCoord);
392  return artCoordTmp;}
393  void set_artCoord(const vpColVector &coord) {
394  pthread_mutex_lock (&mutex_artCoord);
395  artCoord = coord;
396  pthread_mutex_unlock (&mutex_artCoord);}
397 
398  vpColVector get_artVel() {
399  pthread_mutex_lock (&mutex_artVel);
400  vpColVector artVelTmp (artVel);
401  pthread_mutex_unlock (&mutex_artVel);
402  return artVelTmp;}
403  void set_artVel(const vpColVector &vel) {
404  pthread_mutex_lock (&mutex_artVel);
405  artVel = vel;
406  pthread_mutex_unlock (&mutex_artVel);}
407 
408  vpColVector get_velocity() {
409  pthread_mutex_lock (&mutex_velocity);
410  vpColVector velocityTmp = velocity;
411  pthread_mutex_unlock (&mutex_velocity);
412  return velocityTmp;}
413  void set_velocity(const vpColVector &vel) {
414  pthread_mutex_lock (&mutex_velocity);
415  velocity = vel;
416  pthread_mutex_unlock (&mutex_velocity);}
417 
418  void set_displayBusy (const bool &status) {
419  pthread_mutex_lock (&mutex_display);
420  displayBusy = status;
421  pthread_mutex_unlock (&mutex_display);}
422  bool get_displayBusy () {
423  pthread_mutex_lock (&mutex_display);
424  bool status = displayBusy;
425  if (!displayBusy) displayBusy = true;
426  pthread_mutex_unlock (&mutex_display);
427  return status;}
428  #endif
429 
431  virtual void get_fMi(vpHomogeneousMatrix *fMit) = 0;
432 };
433 
434 #endif
435 
436 #endif