Visual Servoing Platform  version 3.2.1 under development (2019-07-18)
vpRobotPioneer.cpp
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  * Interface for Pioneer robots based on Aria 3rd party library.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 #include <visp3/robot/vpRobotException.h>
41 #include <visp3/robot/vpRobotPioneer.h>
42 // Warning: vpMath.h should be included after Aria.h to avoid the build issue:
43 // "/usr/include/Aria/ariaUtil.h:732:21: error: ‘isfinite’ was not declared
44 // in this scope"
45 // This error is due to cmath header included from vpMath.h that makes
46 // isfinite() ambiguous between ::isfinite() and std::isfinite()
47 #include <visp3/core/vpMath.h>
48 
49 #ifdef VISP_HAVE_PIONEER
50 
55 {
56  isInitialized = false;
57 
58  Aria::init();
59 }
60 
65 {
66 #if 0
67  std::cout << "Ending robot thread..." << std::endl;
68  stopRunning();
69 
70  // wait for the thread to stop
71  waitForRunExit();
72 #endif
73 }
74 
102 {
103  init();
104 
105  /*
106  if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
107  vpERROR_TRACE ("Cannot send a velocity to the robot "
108  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
109  throw vpRobotException (vpRobotException::wrongStateError,
110  "Cannot send a velocity to the robot "
111  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
112  } */
113 
114  if (vel.size() != 2) {
115  throw(vpRobotException(vpRobotException::dimensionError, "Velocity vector is not a 2 dimension vector"));
116  }
117 
118  vpColVector vel_max(2);
119  vpColVector vel_sat;
120 
121  if (frame == vpRobot::REFERENCE_FRAME) {
122  vel_max[0] = getMaxTranslationVelocity();
123  vel_max[1] = getMaxRotationVelocity();
124 
125  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
126  this->lock();
127  this->setVel(vel_sat[0] * 1000.); // convert velocity in mm/s
128  this->setRotVel(vpMath::deg(vel_sat[1])); // convert velocity in deg/s
129  this->unlock();
130  } else if (frame == vpRobot::ARTICULAR_FRAME) {
131  vel_max[0] = getMaxTranslationVelocity();
132  vel_max[1] = getMaxTranslationVelocity();
133 
134  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
135  this->lock();
136  // std::cout << "v: " << (vel*1000).t() << " mm/s" << std::endl;
137  this->setVel2(vel_sat[0] * 1000.,
138  vel_sat[1] * 1000.); // convert velocity in mm/s
139  this->unlock();
140  } else {
142  "Cannot send the robot velocity in the specified control frame");
143  }
144 }
145 
155 {
156  if (!isInitialized) {
157  // Start the robot processing cycle running in the background.
158  // True parameter means that if the connection is lost, then the
159  // run loop ends.
160  this->runAsync(true);
161  this->lock();
162  this->enableMotors();
163  this->unlock();
164 
165  isInitialized = true;
166  }
167 }
168 
190 {
191  init();
192  velocity.resize(2);
193 
194  if (frame == vpRobot::ARTICULAR_FRAME) {
195  this->lock();
196  velocity[0] = this->getLeftVel() / 1000.;
197  velocity[1] = this->getRightVel() / 1000;
198  this->unlock();
199  } else if (frame == vpRobot::REFERENCE_FRAME) {
200  this->lock();
201  velocity[0] = this->getVel() / 1000.;
202  velocity[1] = vpMath::rad(this->getRotVel());
203  this->unlock();
204  } else {
206  "Cannot get the robot volocity in the specified control frame");
207  }
208 }
209 
231 {
232  vpColVector velocity;
233  getVelocity(frame, velocity);
234  return velocity;
235 }
236 
237 #elif !defined(VISP_BUILD_SHARED_LIBS)
238 // Work arround to avoid warning: libvisp_robot.a(vpRobotPioneer.cpp.o) has no
239 // symbols
240 void dummy_vpRobotPioneer(){};
241 #endif
Error that can be emited by the vpRobot class and its derivates.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:75
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
virtual ~vpRobotPioneer()
Generic functions for Pioneer mobile robots.
Definition: vpPioneer.h:91
static double rad(double deg)
Definition: vpMath.h:108
static double deg(double rad)
Definition: vpMath.h:101
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310