Visual Servoing Platform  version 3.6.1 under development (2024-11-14)
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 #include <visp3/core/vpDebug.h>
46 
47 #ifdef VISP_HAVE_PIONEER
48 
49 BEGIN_VISP_NAMESPACE
54 {
55  isInitialized = false;
56 
57  Aria::init();
58 }
59 
64 {
65 #if 0
66  std::cout << "Ending robot thread..." << std::endl;
67  stopRunning();
68 
69  // wait for the thread to stop
70  waitForRunExit();
71 #endif
72 }
73 
101 {
102  init();
103 
104  /*
105  if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
106  vpERROR_TRACE ("Cannot send a velocity to the robot "
107  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
108  throw vpRobotException (vpRobotException::wrongStateError,
109  "Cannot send a velocity to the robot "
110  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
111  } */
112 
113  if (vel.size() != 2) {
114  throw(vpRobotException(vpRobotException::dimensionError, "Velocity vector is not a 2 dimension vector"));
115  }
116 
117  vpColVector vel_max(2);
118  vpColVector vel_sat;
119 
120  if (frame == vpRobot::REFERENCE_FRAME) {
121  vel_max[0] = getMaxTranslationVelocity();
122  vel_max[1] = getMaxRotationVelocity();
123 
124  vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
125  this->lock();
126  this->setVel(vel_sat[0] * 1000.); // convert velocity in mm/s
127  this->setRotVel(vpMath::deg(vel_sat[1])); // convert velocity in deg/s
128  this->unlock();
129  }
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  }
141  else {
143  "Cannot send the robot velocity in the specified control frame");
144  }
145 }
146 
156 {
157  if (!isInitialized) {
158  // Start the robot processing cycle running in the background.
159  // True parameter means that if the connection is lost, then the
160  // run loop ends.
161  this->runAsync(true);
162  this->lock();
163  this->enableMotors();
164  this->unlock();
165 
166  isInitialized = true;
167  }
168 }
169 
191 {
192  init();
193  velocity.resize(2);
194 
195  if (frame == vpRobot::ARTICULAR_FRAME) {
196  this->lock();
197  velocity[0] = this->getLeftVel() / 1000.;
198  velocity[1] = this->getRightVel() / 1000;
199  this->unlock();
200  }
201  else if (frame == vpRobot::REFERENCE_FRAME) {
202  this->lock();
203  velocity[0] = this->getVel() / 1000.;
204  velocity[1] = vpMath::rad(this->getRotVel());
205  this->unlock();
206  }
207  else {
209  "Cannot get the robot volocity in the specified control frame");
210  }
211 }
212 
234 {
235  vpColVector velocity;
236  getVelocity(frame, velocity);
237  return velocity;
238 }
239 END_VISP_NAMESPACE
240 #elif !defined(VISP_BUILD_SHARED_LIBS)
241 // Work around to avoid warning: libvisp_robot.a(vpRobotPioneer.cpp.o) has no symbols
242 void dummy_vpRobotPioneer() { };
243 #endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
@ dimensionError
Bad dimension.
Definition: vpException.h:71
static double rad(double deg)
Definition: vpMath.h:129
static double deg(double rad)
Definition: vpMath.h:119
Generic functions for Pioneer mobile robots.
Definition: vpPioneer.h:87
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
virtual ~vpRobotPioneer() VP_OVERRIDE
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
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:164
vpControlFrameType
Definition: vpRobot.h:77
@ REFERENCE_FRAME
Definition: vpRobot.h:78
@ ARTICULAR_FRAME
Definition: vpRobot.h:80
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:274
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:252