ViSP  3.0.0
movePioneer.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Example that shows how to control a Pioneer mobile robot in ViSP.
32  *
33  * Authors:
34  * Fabien Spindler
35  *
36  *****************************************************************************/
37 
38 #include <iostream>
39 
40 #include <visp3/core/vpConfig.h>
41 #include <visp3/core/vpTime.h>
42 #include <visp3/robot/vpRobotPioneer.h>
43 
44 #ifndef VISP_HAVE_PIONEER
45 int main()
46 {
47  std::cout << "\nThis example requires Aria 3rd party library. You should install it.\n"
48  << std::endl;
49  return 0;
50 }
51 
52 #else
53 
66 int main(int argc, char **argv)
67 {
68  try {
69  std::cout << "\nWARNING: this program does no sensing or avoiding of obstacles, \n"
70  "the robot WILL collide with any objects in the way! Make sure the \n"
71  "robot has approximately 3 meters of free space on all sides.\n"
72  << std::endl;
73 
74  vpRobotPioneer robot;
75 
76  ArArgumentParser parser(&argc, argv);
77  parser.loadDefaultArguments();
78 
79  // ArRobotConnector connects to the robot, get some initial data from it such as type and name,
80  // and then loads parameter files for this robot.
81  ArRobotConnector robotConnector(&parser, &robot);
82  if(!robotConnector.connectRobot())
83  {
84  ArLog::log(ArLog::Terse, "Could not connect to the robot.");
85  if(parser.checkHelpAndWarnUnparsed())
86  {
87  Aria::logOptions();
88  Aria::exit(1);
89  }
90  }
91  if (!Aria::parseArgs())
92  {
93  Aria::logOptions();
94  Aria::shutdown();
95  return false;
96  }
97 
98  std::cout << "Robot connected" << std::endl;
99  robot.useSonar(false); // disable the sonar device usage
100 
101  // Robot velocities
102  vpColVector v(2), v_mes(2);
103 
104  for (int i=0; i < 100; i++)
105  {
106  double t = vpTime::measureTimeMs();
107 
108  v = 0;
109  v[0] = i/1000.; // Translational velocity in m/s
110  //v[1] = vpMath::rad(i/5.); // Rotational velocity in rad/sec
112 
113  v_mes = robot.getVelocity(vpRobot::REFERENCE_FRAME);
114  std::cout << "Trans. vel= " << v_mes[0] << " m/s, Rot. vel=" << vpMath::deg(v_mes[1]) << " deg/s" << std::endl;
115  v_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME);
116  std::cout << "Left wheel vel= " << v_mes[0] << " m/s, Right wheel vel=" << v_mes[1] << " m/s" << std::endl;
117  std::cout << "Battery=" << robot.getBatteryVoltage() << std::endl;
118 
119  vpTime::wait(t, 40);
120  }
121 
122  ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping.");
123  robot.lock();
124  robot.stop();
125  robot.unlock();
126  ArUtil::sleep(1000);
127 
128  robot.lock();
129  ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
130  robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage());
131  robot.unlock();
132 
133  std::cout << "Ending robot thread..." << std::endl;
134  robot.stopRunning();
135 
136  // wait for the thread to stop
137  robot.waitForRunExit();
138 
139  // exit
140  ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting.");
141  return 0;
142  }
143  catch(vpException e) {
144  std::cout << "Catch an exception: " << e << std::endl;
145  return 1;
146  }
147 }
148 
149 #endif
150 
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
void useSonar(bool usage)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
error that can be emited by ViSP classes.
Definition: vpException.h:73
Interface for Pioneer mobile robots based on Aria 3rd party library.
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
static double deg(double rad)
Definition: vpMath.h:97
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)