ViSP  2.10.0
vpRobotPioneer.cpp
1 /****************************************************************************
2  *
3  * $Id: vpRobotPioneer.cpp 4574 2014-01-09 08:48:51Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 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 
53 {
54  isInitialized = false;
55 
56  Aria::init();
57 }
58 
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 
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  {
140  "Cannot send the robot velocity in the specified control frame");
141  }
142 }
143 
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 
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 {
206  "Cannot get the robot volocity in the specified control frame");
207  }
208 }
209 
229 {
230  vpColVector velocity;
231  getVelocity(frame, velocity);
232  return velocity;
233 }
234 
235 #endif
236 
Error that can be emited by the vpRobot class and its derivates.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:254
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:167
vpControlFrameType
Definition: vpRobot.h:78
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:279
unsigned int size() const
Definition: vpColVector.h:204
virtual ~vpRobotPioneer()
Generic functions for Pioneer mobile robots.
Definition: vpPioneer.h:94
static double rad(double deg)
Definition: vpMath.h:100
static double deg(double rad)
Definition: vpMath.h:93
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:98