Visual Servoing Platform version 3.5.0
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
240void dummy_vpRobotPioneer(){};
241#endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
@ dimensionError
Bad dimension.
Definition: vpException.h:95
static double rad(double deg)
Definition: vpMath.h:110
static double deg(double rad)
Definition: vpMath.h:103
Generic functions for Pioneer mobile robots.
Definition: vpPioneer.h:92
Error that can be emited by the vpRobot class and its derivates.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
virtual ~vpRobotPioneer()
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:163
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251