Visual Servoing Platform  version 3.4.0
movePioneer.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  * Example that shows how to control a Pioneer mobile robot in ViSP.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <iostream>
40 
41 #include <visp3/core/vpConfig.h>
42 #include <visp3/core/vpTime.h>
43 #include <visp3/robot/vpRobotPioneer.h>
44 
45 #ifndef VISP_HAVE_PIONEER
46 int main()
47 {
48  std::cout << "\nThis example requires Aria 3rd party library. You should "
49  "install it.\n"
50  << std::endl;
51  return EXIT_SUCCESS;
52 }
53 
54 #else
55 
68 int main(int argc, char **argv)
69 {
70  try {
71  std::cout << "\nWARNING: this program does no sensing or avoiding of "
72  "obstacles, \n"
73  "the robot WILL collide with any objects in the way! Make sure "
74  "the \n"
75  "robot has approximately 3 meters of free space on all sides.\n"
76  << std::endl;
77 
78  vpRobotPioneer robot;
79 
80  ArArgumentParser parser(&argc, argv);
81  parser.loadDefaultArguments();
82 
83  // ArRobotConnector connects to the robot, get some initial data from it
84  // such as type and name, and then loads parameter files for this robot.
85  ArRobotConnector robotConnector(&parser, &robot);
86  if (!robotConnector.connectRobot()) {
87  ArLog::log(ArLog::Terse, "Could not connect to the robot.");
88  if (parser.checkHelpAndWarnUnparsed()) {
89  Aria::logOptions();
90  Aria::exit(1);
91  }
92  }
93  if (!Aria::parseArgs()) {
94  Aria::logOptions();
95  Aria::shutdown();
96  return false;
97  }
98 
99  std::cout << "Robot connected" << std::endl;
100  robot.useSonar(false); // disable the sonar device usage
101 
102  // Robot velocities
103  vpColVector v(2), v_mes(2);
104 
105  for (int i = 0; i < 100; i++) {
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,
130  "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. "
131  "Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
132  robot.getX(), robot.getY(), robot.getTh(), robot.getVel(), robot.getRotVel(), robot.getBatteryVoltage());
133  robot.unlock();
134 
135  std::cout << "Ending robot thread..." << std::endl;
136  robot.stopRunning();
137 
138  // wait for the thread to stop
139  robot.waitForRunExit();
140 
141  // exit
142  ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting.");
143  return EXIT_SUCCESS;
144  } catch (const vpException &e) {
145  std::cout << "Catch an exception: " << e << std::endl;
146  return EXIT_FAILURE;
147  }
148 }
149 
150 #endif
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:173
void useSonar(bool usage)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
error that can be emited by ViSP classes.
Definition: vpException.h:71
Interface for Pioneer mobile robots based on Aria 3rd party library.
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
static double deg(double rad)
Definition: vpMath.h:103
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)