ViSP
Main Page
Related Pages
Modules
Classes
Examples
All
Classes
Functions
Variables
Enumerations
Enumerator
Friends
Groups
Pages
vpRobotPioneer.cpp
1
/****************************************************************************
2
*
3
* $Id: vpRobotPioneer.cpp 4056 2013-01-05 13:04:42Z fspindle $
4
*
5
* This file is part of the ViSP software.
6
* Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7
*
8
* This software is free software; you can redistribute it and/or
9
* modify it under the terms of the GNU General Public License
10
* ("GPL") version 2 as published by the Free Software Foundation.
11
* See the file LICENSE.txt at the root directory of this source
12
* distribution for additional information about the GNU GPL.
13
*
14
* For using ViSP with software that can not be combined with the GNU
15
* GPL, please contact INRIA about acquiring a ViSP Professional
16
* Edition License.
17
*
18
* See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19
*
20
* This software was developed at:
21
* INRIA Rennes - Bretagne Atlantique
22
* Campus Universitaire de Beaulieu
23
* 35042 Rennes Cedex
24
* France
25
* http://www.irisa.fr/lagadic
26
*
27
* If you have questions regarding the use of this file, please contact
28
* INRIA at visp@inria.fr
29
*
30
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32
*
33
*
34
* Description:
35
* Interface for Pioneer robots based on Aria 3rd party library.
36
*
37
* Authors:
38
* Fabien Spindler
39
*
40
*****************************************************************************/
41
42
#include <visp/vpConfig.h>
43
#include <visp/vpMath.h>
44
#include <visp/vpRobotException.h>
45
#include <visp/vpRobotPioneer.h>
46
47
#ifdef VISP_HAVE_PIONEER
48
52
vpRobotPioneer::vpRobotPioneer
() :
vpPioneer
(), ArRobot()
53
{
54
isInitialized
=
false
;
55
56
Aria::init
();
57
}
58
62
vpRobotPioneer::~vpRobotPioneer
()
63
{
64
#if 0
65
std::cout <<
"Ending robot thread..."
<< std::endl;
66
stopRunning();
67
68
// wait for the thread to stop
69
waitForRunExit();
70
#endif
71
}
72
94
void
vpRobotPioneer::setVelocity
(
const
vpRobot::vpControlFrameType
frame,
const
vpColVector
&vel)
95
{
96
init
();
97
98
/*
99
if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
100
vpERROR_TRACE ("Cannot send a velocity to the robot "
101
"use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
102
throw vpRobotException (vpRobotException::wrongStateError,
103
"Cannot send a velocity to the robot "
104
"use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
105
} */
106
107
if
(vel.
size
() != 2)
108
{
109
throw
(
vpRobotException
(
vpRobotException::dimensionError
,
"Velocity vector is not a 2 dimension vector"
));
110
}
111
112
vpColVector
vel_max(2);
113
vpColVector
vel_sat;
114
115
if
(frame ==
vpRobot::REFERENCE_FRAME
)
116
{
117
vel_max[0] =
getMaxTranslationVelocity
();
118
vel_max[1] =
getMaxRotationVelocity
();
119
120
vel_sat =
vpRobot::saturateVelocities
(vel, vel_max,
true
);
121
this->lock();
122
this->setVel(vel_sat[0]*1000.);
// convert velocity in mm/s
123
this->setRotVel(
vpMath::deg
(vel_sat[1]) );
// convert velocity in deg/s
124
this->unlock();
125
}
126
else
if
(frame ==
vpRobot::ARTICULAR_FRAME
)
127
{
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., vel_sat[1]*1000.);
// convert velocity in mm/s
135
this->unlock();
136
}
137
else
138
{
139
throw
vpRobotException
(
vpRobotException::wrongStateError
,
140
"Cannot send the robot velocity in the specified control frame"
);
141
}
142
}
143
151
void
vpRobotPioneer::init
()
152
{
153
if
( !
isInitialized
)
154
{
155
// Start the robot processing cycle running in the background.
156
// True parameter means that if the connection is lost, then the
157
// run loop ends.
158
this->runAsync(
true
);
159
this->lock();
160
this->enableMotors();
161
this->unlock();
162
163
isInitialized
=
true
;
164
}
165
}
166
185
void
vpRobotPioneer::getVelocity
(
const
vpRobot::vpControlFrameType
frame,
vpColVector
& velocity)
186
{
187
init
();
188
velocity.
resize
(2);
189
190
if
(frame ==
vpRobot::ARTICULAR_FRAME
)
191
{
192
this->lock();
193
velocity[0] = this->getLeftVel() / 1000.;
194
velocity[1] = this->getRightVel() / 1000;
195
this->unlock();
196
}
197
else
if
(frame ==
vpRobot::REFERENCE_FRAME
)
198
{
199
this->lock();
200
velocity[0] = this->getVel() / 1000.;
201
velocity[1] =
vpMath::rad
( this->getRotVel() );
202
this->unlock();
203
}
204
else
{
205
throw
vpRobotException
(
vpRobotException::wrongStateError
,
206
"Cannot get the robot volocity in the specified control frame"
);
207
}
208
}
209
228
vpColVector
vpRobotPioneer::getVelocity
(
const
vpRobot::vpControlFrameType
frame)
229
{
230
vpColVector
velocity;
231
getVelocity
(frame, velocity);
232
return
velocity;
233
}
234
235
#endif
236
src
robot
real-robot
pioneer
vpRobotPioneer.cpp
Generated on Wed Jun 12 2013 05:39:46 for ViSP by
1.8.1.2