Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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 } else if (frame == vpRobot::ARTICULAR_FRAME) {
128 vel_max[0] = getMaxTranslationVelocity();
129 vel_max[1] = getMaxTranslationVelocity();
130
131 vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
132 this->lock();
133 // std::cout << "v: " << (vel*1000).t() << " mm/s" << std::endl;
134 this->setVel2(vel_sat[0] * 1000.,
135 vel_sat[1] * 1000.); // convert velocity in mm/s
136 this->unlock();
137 } else {
139 "Cannot send the robot velocity in the specified control frame");
140 }
141}
142
152{
153 if (!isInitialized) {
154 // Start the robot processing cycle running in the background.
155 // True parameter means that if the connection is lost, then the
156 // run loop ends.
157 this->runAsync(true);
158 this->lock();
159 this->enableMotors();
160 this->unlock();
161
162 isInitialized = true;
163 }
164}
165
187{
188 init();
189 velocity.resize(2);
190
191 if (frame == vpRobot::ARTICULAR_FRAME) {
192 this->lock();
193 velocity[0] = this->getLeftVel() / 1000.;
194 velocity[1] = this->getRightVel() / 1000;
195 this->unlock();
196 } else if (frame == vpRobot::REFERENCE_FRAME) {
197 this->lock();
198 velocity[0] = this->getVel() / 1000.;
199 velocity[1] = vpMath::rad(this->getRotVel());
200 this->unlock();
201 } else {
203 "Cannot get the robot volocity in the specified control frame");
204 }
205}
206
228{
229 vpColVector velocity;
230 getVelocity(frame, velocity);
231 return velocity;
232}
233
234#elif !defined(VISP_BUILD_SHARED_LIBS)
235// Work around to avoid warning: libvisp_robot.a(vpRobotPioneer.cpp.o) has no
236// symbols
237void dummy_vpRobotPioneer(){};
238#endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
@ dimensionError
Bad dimension.
Definition vpException.h:83
static double rad(double deg)
Definition vpMath.h:116
static double deg(double rad)
Definition vpMath.h:106
Generic functions for Pioneer mobile robots.
Definition vpPioneer.h:89
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
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:160
vpControlFrameType
Definition vpRobot.h:73
@ REFERENCE_FRAME
Definition vpRobot.h:74
@ ARTICULAR_FRAME
Definition vpRobot.h:76
double getMaxRotationVelocity(void) const
Definition vpRobot.cpp:270
double getMaxTranslationVelocity(void) const
Definition vpRobot.cpp:248