Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
vpRobotWireFrameSimulator.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Basic class used to make robot simulators.
33  *
34  * Authors:
35  * Nicolas Melchior
36  *
37  *****************************************************************************/
38 
39 #ifndef vpRobotWireFrameSimulator_HH
40 #define vpRobotWireFrameSimulator_HH
41 
47 #include <visp3/core/vpConfig.h>
48 
49 #if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
50 
51 #include <cmath> // std::fabs
52 #include <limits> // numeric_limits
53 #if defined(_WIN32)
54 // Include WinSock2.h before windows.h to ensure that winsock.h is not
55 // included by windows.h since winsock.h and winsock2.h are incompatible
56 #include <WinSock2.h>
57 #include <windows.h>
58 #elif defined(VISP_HAVE_PTHREAD)
59 #include <pthread.h>
60 #endif
61 
62 #include <visp3/gui/vpDisplayD3D.h>
63 #include <visp3/gui/vpDisplayGDI.h>
64 #include <visp3/gui/vpDisplayGTK.h>
65 #include <visp3/gui/vpDisplayOpenCV.h>
66 #include <visp3/gui/vpDisplayX.h>
67 #include <visp3/robot/vpRobot.h>
68 #include <visp3/robot/vpRobotSimulator.h>
69 #include <visp3/robot/vpWireFrameSimulator.h>
70 
89 {
90 public:
92 
93  typedef enum { MODEL_3D, MODEL_DH } vpDisplayRobotType;
94 
95 protected:
97  double tcur;
99  double tprev;
100 
102  Bound_scene *robotArms;
103 
105  unsigned int size_fMi;
113 
120 
121 #if defined(_WIN32)
122  HANDLE hThread;
123  HANDLE mutex_fMi;
124  HANDLE mutex_artVel;
125  HANDLE mutex_artCoord;
126  HANDLE mutex_velocity;
127  HANDLE mutex_display;
128 #elif defined(VISP_HAVE_PTHREAD)
129  pthread_t thread;
130  pthread_attr_t attr;
131  pthread_mutex_t mutex_fMi;
132  pthread_mutex_t mutex_artVel;
133  pthread_mutex_t mutex_artCoord;
134  pthread_mutex_t mutex_velocity;
135  pthread_mutex_t mutex_display;
136 #endif
137 
139 
141  bool robotStop;
145  unsigned int jointLimitArt;
148 
151 
152 #if defined VISP_HAVE_X11
154 #elif defined VISP_HAVE_GDI
155  vpDisplayGDI display;
156 #elif defined VISP_HAVE_OPENCV
157  vpDisplayOpenCV display;
158 #elif defined VISP_HAVE_D3D9
159  vpDisplayD3D display;
160 #elif defined VISP_HAVE_GTK
161  vpDisplayGTK display;
162 #endif
163 
164  vpDisplayRobotType displayType;
165 
172 
176 
177  bool verbose_;
178 
179  // private:
180  //#ifndef DOXYGEN_SHOULD_SKIP_THIS
181  // vpRobotWireFrameSimulator(const vpRobotWireFrameSimulator &)
182  // : vpWireFrameSimulator(), vpRobotSimulator(),
183  // I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL),
184  // artCoord(), artVel(), velocity(),
185  // #if defined(_WIN32)
186  // #elif defined(VISP_HAVE_PTHREAD)
187  // thread(), attr(),
188  // #endif
189  // mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(),
190  // mutex_display(), displayBusy(false), robotStop(false),
191  // jointLimit(false), jointLimitArt(false),
192  // singularityManagement(true), cameraParam(),
193  // #if defined(VISP_HAVE_DISPLAY)
194  // display(),
195  // #endif
196  // displayType(MODEL_3D), displayAllowed(true),
197  // constantSamplingTimeMode(false), setVelocityCalled(false),
198  // verbose_(false)
199  // {
200  // throw vpException(vpException::functionNotImplementedError, "Not
201  // implemented!");
202  // }
203  // vpRobotWireFrameSimulator &operator=(const vpRobotWireFrameSimulator
204  // &){
205  // throw vpException(vpException::functionNotImplementedError, "Not
206  // implemented!"); return *this;
207  // }
208  //#endif
209 
210 public:
212  explicit vpRobotWireFrameSimulator(bool display);
213  virtual ~vpRobotWireFrameSimulator();
214 
223  {
224  // if(px_ext != 1 && py_ext != 1)
225  // we assume px_ext and py_ext > 0
226  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
227  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
228  return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
229  else {
230  unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
231  return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
232  }
233  }
242  {
244  }
245 
246  void getInternalView(vpImage<vpRGBa> &I);
247  void getInternalView(vpImage<unsigned char> &I);
248 
255  vpHomogeneousMatrix get_fMo() const { return fMo; }
256 
257  /* Display functions */
258  void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
259  void initScene(const char *obj, const char *desiredObject);
260  void initScene(const vpSceneObject &obj);
261  void initScene(const char *obj);
262 
268  void setCameraColor(const vpColor &col) { camColor = col; }
269 
279  inline void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode)
280  {
281  constantSamplingTimeMode = _constantSamplingTimeMode;
282  }
283 
290  void setCurrentViewColor(const vpColor &col) { curColor = col; }
291 
298  void setDesiredViewColor(const vpColor &col) { desColor = col; }
299 
306  {
308  }
309 
317  inline void setDisplayRobotType(const vpDisplayRobotType dispType) { displayType = dispType; }
325  {
327  }
331  void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; }
332 
343  inline void setSamplingTime(const double &delta_t)
344  {
345  if (delta_t < static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3)) {
346  this->delta_t_ = static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3);
347  } else {
348  this->delta_t_ = delta_t;
349  }
350  }
352  void setSingularityManagement(bool sm) { singularityManagement = sm; }
353 
357  void setVerbose(bool verbose) { this->verbose_ = verbose; }
358 
364  void set_fMo(const vpHomogeneousMatrix &fMo_) { this->fMo = fMo_; }
366 
367 protected:
374 #if defined(_WIN32)
375  static DWORD WINAPI launcher(LPVOID lpParam)
376  {
377  (static_cast<vpRobotWireFrameSimulator *>(lpParam))->updateArticularPosition();
378  return 0;
379  }
380 #elif defined(VISP_HAVE_PTHREAD)
381  static void *launcher(void *arg)
382  {
383  (reinterpret_cast<vpRobotWireFrameSimulator *>(arg))->updateArticularPosition();
384  // pthread_exit((void*) 0);
385  return NULL;
386  }
387 #endif
388 
389  /* Robot functions */
390  void init() { ; }
393  virtual void updateArticularPosition() = 0;
395  virtual int isInJointLimit() = 0;
398  virtual void computeArticularVelocity() = 0;
399 
400  /* Display functions */
401  void initDisplay() { ; }
402  virtual void initArms() = 0;
403 
404 #if defined(_WIN32)
405  vpColVector get_artCoord() const
406  {
407 #if defined(WINRT_8_1)
408  WaitForSingleObjectEx(mutex_artCoord, INFINITE, FALSE);
409 #else // pure win32
410  WaitForSingleObject(mutex_artCoord, INFINITE);
411 #endif
412  vpColVector artCoordTmp(6);
413  artCoordTmp = artCoord;
414  ReleaseMutex(mutex_artCoord);
415  return artCoordTmp;
416  }
417  void set_artCoord(const vpColVector &coord)
418  {
419 #if defined(WINRT_8_1)
420  WaitForSingleObjectEx(mutex_artCoord, INFINITE, FALSE);
421 #else // pure win32
422  WaitForSingleObject(mutex_artCoord, INFINITE);
423 #endif
424  artCoord = coord;
425  ReleaseMutex(mutex_artCoord);
426  }
427 
428  vpColVector get_artVel() const
429  {
430 #if defined(WINRT_8_1)
431  WaitForSingleObjectEx(mutex_artVel, INFINITE, FALSE);
432 #else // pure win32
433  WaitForSingleObject(mutex_artVel, INFINITE);
434 #endif
435  vpColVector artVelTmp(artVel);
436  ReleaseMutex(mutex_artVel);
437  return artVelTmp;
438  }
439  void set_artVel(const vpColVector &vel)
440  {
441 #if defined(WINRT_8_1)
442  WaitForSingleObjectEx(mutex_artVel, INFINITE, FALSE);
443 #else // pure win32
444  WaitForSingleObject(mutex_artVel, INFINITE);
445 #endif
446  artVel = vel;
447  ReleaseMutex(mutex_artVel);
448  }
449 
450  vpColVector get_velocity()
451  {
452 #if defined(WINRT_8_1)
453  WaitForSingleObjectEx(mutex_velocity, INFINITE, FALSE);
454 #else // pure win32
455  WaitForSingleObject(mutex_velocity, INFINITE);
456 #endif
457  vpColVector velocityTmp = velocity;
458  ReleaseMutex(mutex_velocity);
459  return velocityTmp;
460  }
461  void set_velocity(const vpColVector &vel)
462  {
463 #if defined(WINRT_8_1)
464  WaitForSingleObjectEx(mutex_velocity, INFINITE, FALSE);
465 #else // pure win32
466  WaitForSingleObject(mutex_velocity, INFINITE);
467 #endif
468  velocity = vel;
469  ReleaseMutex(mutex_velocity);
470  }
471 
472  void set_displayBusy(const bool &status)
473  {
474 #if defined(WINRT_8_1)
475  WaitForSingleObjectEx(mutex_display, INFINITE, FALSE);
476 #else // pure win32
477  WaitForSingleObject(mutex_display, INFINITE);
478 #endif
479  displayBusy = status;
480  ReleaseMutex(mutex_display);
481  }
482  bool get_displayBusy()
483  {
484 #if defined(WINRT_8_1)
485  WaitForSingleObjectEx(mutex_display, INFINITE, FALSE);
486 #else // pure win32
487  WaitForSingleObject(mutex_display, INFINITE);
488 #endif
489  bool status = displayBusy;
490  if (!displayBusy)
491  displayBusy = true;
492  ReleaseMutex(mutex_display);
493  return status;
494  }
495 
496 #elif defined(VISP_HAVE_PTHREAD)
498  {
499  pthread_mutex_lock(&mutex_artCoord);
500  vpColVector artCoordTmp(6);
501  artCoordTmp = artCoord;
502  pthread_mutex_unlock(&mutex_artCoord);
503  return artCoordTmp;
504  }
505  void set_artCoord(const vpColVector &coord)
506  {
507  pthread_mutex_lock(&mutex_artCoord);
508  artCoord = coord;
509  pthread_mutex_unlock(&mutex_artCoord);
510  }
511 
513  {
514  pthread_mutex_lock(&mutex_artVel);
515  vpColVector artVelTmp(artVel);
516  pthread_mutex_unlock(&mutex_artVel);
517  return artVelTmp;
518  }
519  void set_artVel(const vpColVector &vel)
520  {
521  pthread_mutex_lock(&mutex_artVel);
522  artVel = vel;
523  pthread_mutex_unlock(&mutex_artVel);
524  }
525 
527  {
528  pthread_mutex_lock(&mutex_velocity);
529  vpColVector velocityTmp = velocity;
530  pthread_mutex_unlock(&mutex_velocity);
531  return velocityTmp;
532  }
533  void set_velocity(const vpColVector &vel)
534  {
535  pthread_mutex_lock(&mutex_velocity);
536  velocity = vel;
537  pthread_mutex_unlock(&mutex_velocity);
538  }
539 
540  void set_displayBusy(const bool &status)
541  {
542  pthread_mutex_lock(&mutex_display);
543  displayBusy = status;
544  pthread_mutex_unlock(&mutex_display);
545  }
547  {
548  pthread_mutex_lock(&mutex_display);
549  bool status = displayBusy;
550  if (!displayBusy)
551  displayBusy = true;
552  pthread_mutex_unlock(&mutex_display);
553  return status;
554  }
555 #endif
556 
559  virtual void get_fMi(vpHomogeneousMatrix *fMit) = 0;
561 };
562 
563 #endif
564 #endif
void setCurrentViewColor(const vpColor &col)
void setDisplayRobotType(const vpDisplayRobotType dispType)
vpHomogeneousMatrix getExternalCameraPosition() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void * launcher(void *arg)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
Class to define colors available for display functionnalities.
Definition: vpColor.h:119
void set_fMo(const vpHomogeneousMatrix &fMo_)
void setDesiredViewColor(const vpColor &col)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
void setGraphicsThickness(unsigned int thickness)
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
vpHomogeneousMatrix fMo
void setCameraColor(const vpColor &col)
vpHomogeneousMatrix get_fMo() const
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:143
vpCameraParameters getExternalCameraParameters() const
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed...
Definition: vpDisplayD3D.h:106
vpHomogeneousMatrix get_cMo() const
This class aims to be a basis used to create all the robot simulators.
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
void set_displayBusy(const bool &status)
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
void setSamplingTime(const double &delta_t)
Generic class defining intrinsic camera parameters.
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:137
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:151
void set_velocity(const vpColVector &vel)
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
This class aims to be a basis used to create all the simulators of robots.
unsigned int getHeight() const
Definition: vpImage.h:186
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode)
void set_artCoord(const vpColVector &coord)
VISP_EXPORT double getMinTimeForUsleepCall()
Definition: vpTime.cpp:86
void set_artVel(const vpColVector &vel)
unsigned int getWidth() const
Definition: vpImage.h:244
vpHomogeneousMatrix getExternalCameraPosition() const