Visual Servoing Platform version 3.5.0
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{
90public:
92
93 typedef enum { MODEL_3D, MODEL_DH } vpDisplayRobotType;
94
95protected:
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
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
165
172
176
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
210public:
212 explicit vpRobotWireFrameSimulator(bool display);
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
367protected:
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
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:107
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:135
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
Implementation of an homogeneous matrix and operations on such kind of matrices.
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:153
This class aims to be a basis used to create all the robot simulators.
This class aims to be a basis used to create all the simulators of robots.
virtual void computeArticularVelocity()=0
void setDisplayRobotType(const vpDisplayRobotType dispType)
virtual int isInJointLimit()=0
void setGraphicsThickness(unsigned int thickness)
void set_velocity(const vpColVector &vel)
void setCurrentViewColor(const vpColor &col)
void set_displayBusy(const bool &status)
void setDesiredViewColor(const vpColor &col)
vpHomogeneousMatrix getExternalCameraPosition() const
vpHomogeneousMatrix get_fMo() const
void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode)
void set_artCoord(const vpColVector &coord)
void setCameraColor(const vpColor &col)
virtual void initArms()=0
vpCameraParameters getExternalCameraParameters() const
virtual void updateArticularPosition()=0
static void * launcher(void *arg)
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
virtual void get_fMi(vpHomogeneousMatrix *fMit)=0
void set_fMo(const vpHomogeneousMatrix &fMo_)
void setSamplingTime(const double &delta_t)
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
void set_artVel(const vpColVector &vel)
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
vpHomogeneousMatrix getExternalCameraPosition() const
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
vpHomogeneousMatrix get_cMo() const
vpHomogeneousMatrix fMo
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
VISP_EXPORT double getMinTimeForUsleepCall()