Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
vpRobotPioneer.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #include <visp3/core/vpConfig.h>
37 #include <visp3/robot/vpRobotException.h>
38 #include <visp3/robot/vpRobotPioneer.h>
39 // Warning: vpMath.h should be included after Aria.h to avoid the build issue:
40 // "/usr/include/Aria/ariaUtil.h:732:21: error: ‘isfinite’ was not declared
41 // in this scope"
42 // This error is due to cmath header included from vpMath.h that makes
43 // isfinite() ambiguous between ::isfinite() and std::isfinite()
44 #include <visp3/core/vpMath.h>
45 
46 #ifdef VISP_HAVE_PIONEER
47 
52 {
53  isInitialized = false;
54 
55  Aria::init();
56 }
57 
62 {
63 #if 0
64  std::cout << "Ending robot thread..." << std::endl;
65  stopRunning();
66 
67  // wait for the thread to stop
68  waitForRunExit();
69 #endif
70 }
71 
99 {
100  init();
101 
102  /*
103  if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
104  vpERROR_TRACE ("Cannot send a velocity to the robot "
105  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
106  throw vpRobotException (vpRobotException::wrongStateError,
107  "Cannot send a velocity to the robot "
108  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
109  } */
110 
111  if (vel.size() != 2) {
112  throw(vpRobotException(vpRobotException::dimensionError, "Velocity vector is not a 2 dimension vector"));
113  }
114 
115  vpColVector vel_max(2);
116  vpColVector vel_sat;
117 
118  if (frame == vpRobot::REFERENCE_FRAME) {
119  vel_max[0] = getMaxTranslationVelocity();
120  vel_max[1] = getMaxRotationVelocity();
121 
122  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
123  this->lock();
124  this->setVel(vel_sat[0] * 1000.); // convert velocity in mm/s
125  this->setRotVel(vpMath::deg(vel_sat[1])); // convert velocity in deg/s
126  this->unlock();
127  }
128  else if (frame == vpRobot::ARTICULAR_FRAME) {
129  vel_max[0] = getMaxTranslationVelocity();
130  vel_max[1] = getMaxTranslationVelocity();
131 
132  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
133  this->lock();
134  // std::cout << "v: " << (vel*1000).t() << " mm/s" << std::endl;
135  this->setVel2(vel_sat[0] * 1000.,
136  vel_sat[1] * 1000.); // convert velocity in mm/s
137  this->unlock();
138  }
139  else {
141  "Cannot send the robot velocity in the specified control frame");
142  }
143 }
144 
154 {
155  if (!isInitialized) {
156  // Start the robot processing cycle running in the background.
157  // True parameter means that if the connection is lost, then the
158  // run loop ends.
159  this->runAsync(true);
160  this->lock();
161  this->enableMotors();
162  this->unlock();
163 
164  isInitialized = true;
165  }
166 }
167 
189 {
190  init();
191  velocity.resize(2);
192 
193  if (frame == vpRobot::ARTICULAR_FRAME) {
194  this->lock();
195  velocity[0] = this->getLeftVel() / 1000.;
196  velocity[1] = this->getRightVel() / 1000;
197  this->unlock();
198  }
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  }
205  else {
207  "Cannot get the robot volocity in the specified control frame");
208  }
209 }
210 
232 {
233  vpColVector velocity;
234  getVelocity(frame, velocity);
235  return velocity;
236 }
237 
238 #elif !defined(VISP_BUILD_SHARED_LIBS)
239 // Work around to avoid warning: libvisp_robot.a(vpRobotPioneer.cpp.o) has no symbols
240 void dummy_vpRobotPioneer() { };
241 #endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:286
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
@ dimensionError
Bad dimension.
Definition: vpException.h:83
static double rad(double deg)
Definition: vpMath.h:127
static double deg(double rad)
Definition: vpMath.h:117
Generic functions for Pioneer mobile robots.
Definition: vpPioneer.h:85
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
virtual ~vpRobotPioneer() vp_override
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:160
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:270
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:248