Visual Servoing Platform  version 3.0.1
sonarPioneerReader.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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/robot/vpRobotPioneer.h> // Include before vpDisplayX to avoid build issues
41 #include <visp3/core/vpConfig.h>
42 #include <visp3/core/vpDisplay.h>
43 #include <visp3/gui/vpDisplayGDI.h>
44 #include <visp3/gui/vpDisplayX.h>
45 #include <visp3/core/vpImage.h>
46 #include <visp3/core/vpIoTools.h>
47 #include <visp3/io/vpImageIo.h>
48 #include <visp3/core/vpTime.h>
49 
50 #ifndef VISP_HAVE_PIONEER
51 int main()
52 {
53  std::cout << "\nThis example requires Aria 3rd party library. You should install it.\n"
54  << std::endl;
55  return 0;
56 }
57 
58 #else
59 
60 ArSonarDevice sonar;
61 vpRobotPioneer *robot;
62 #if defined(VISP_HAVE_X11)
63 vpDisplayX *d;
64 #elif defined (VISP_HAVE_GDI)
65 vpDisplayGDI *d;
66 #endif
68 static bool isInitialized = false;
69 static int half_size = 256*2;
70 
71 void sonarPrinter(void)
72 {
73  fprintf(stdout, "in sonarPrinter()\n"); fflush(stdout);
74  double scale = (double)half_size / (double)sonar.getMaxRange();
75 
76  /*
77  ArSonarDevice *sd;
78 
79  std::list<ArPoseWithTime *>::iterator it;
80  std::list<ArPoseWithTime *> *readings;
81  ArPose *pose;
82 
83  sd = (ArSonarDevice *)robot->findRangeDevice("sonar");
84  if (sd != NULL)
85  {
86  sd->lockDevice();
87  readings = sd->getCurrentBuffer();
88  if (readings != NULL)
89  {
90  for (it = readings->begin(); it != readings->end(); ++it)
91  {
92  pose = (*it);
93  //pose->log();
94  }
95  }
96  sd->unlockDevice();
97  }
98 */
99  double range;
100  double angle;
101 
102  /*
103  * example to show how to find closest readings within polar sections
104  */
105  printf("Closest readings within polar sections:\n");
106 
107  double start_angle = -45;
108  double end_angle = 45;
109  range = sonar.currentReadingPolar(start_angle, end_angle, &angle);
110  printf(" front quadrant: %5.0f ", range);
111  //if (range != sonar.getMaxRange())
112  if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
113  printf("%3.0f ", angle);
114  printf("\n");
115 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
116  //if (isInitialized && range != sonar.getMaxRange())
117  if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
118  {
119  double x = range * cos(vpMath::rad(angle)); // position of the obstacle in the sensor frame
120  double y = range * sin(vpMath::rad(angle));
121 
122  // Conversion in pixels so that the robot frame is in the middle of the image
123  double j = -y * scale + half_size; // obstacle position
124  double i = -x * scale + half_size;
125 
127  vpDisplay::displayLine(I, half_size, half_size, 0, 0, vpColor::red, 5);
128  vpDisplay::displayLine(I, half_size, half_size, 0, 2*half_size-1, vpColor::red, 5);
129  vpDisplay::displayLine(I, half_size, half_size, i, j, vpColor::green, 3);
131  }
132 #endif
133 
134  range = sonar.currentReadingPolar(-135, -45, &angle);
135  printf(" right quadrant: %5.0f ", range);
136  //if (range != sonar.getMaxRange())
137  if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
138  printf("%3.0f ", angle);
139  printf("\n");
140 
141  range = sonar.currentReadingPolar(45, 135, &angle);
142  printf(" left quadrant: %5.0f ", range);
143  //if (range != sonar.getMaxRange())
144  if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
145  printf("%3.0f ", angle);
146  printf("\n");
147 
148  range = sonar.currentReadingPolar(-135, 135, &angle);
149  printf(" back quadrant: %5.0f ", range);
150  //if (range != sonar.getMaxRange())
151  if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
152  printf("%3.0f ", angle);
153  printf("\n");
154 
155  /*
156  * example to show how get all sonar sensor data
157  */
158  ArSensorReading *reading;
159  for (int sensor = 0; sensor < robot->getNumSonar(); sensor++)
160  {
161  reading = robot->getSonarReading(sensor);
162  if (reading != NULL)
163  {
164  angle = reading->getSensorTh();
165  range = reading->getRange();
166  double sx = reading->getSensorX(); // position of the sensor in the robot frame
167  double sy = reading->getSensorY();
168  double ox = range * cos(vpMath::rad(angle)); // position of the obstacle in the sensor frame
169  double oy = range * sin(vpMath::rad(angle));
170  double x = sx + ox; // position of the obstacle in the robot frame
171  double y = sy + oy;
172 
173  // Conversion in pixels so that the robot frame is in the middle of the image
174  double sj = -sy * scale + half_size; // sensor position
175  double si = -sx * scale + half_size;
176  double j = -y * scale + half_size; // obstacle position
177  double i = -x * scale + half_size;
178 
179  // printf("%d x: %.1f y: %.1f th: %.1f d: %d\n", sensor, reading->getSensorX(),
180  // reading->getSensorY(), reading->getSensorTh(), reading->getRange());
181 
182 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
183  //if (isInitialized && range != sonar.getMaxRange())
184  if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
185  {
186  vpDisplay::displayLine(I, si, sj, i, j, vpColor::blue, 2);
187  vpDisplay::displayCross(I, si, sj, 7, vpColor::blue);
188  char legend[15];
189  sprintf(legend, "%d: %1.2fm", sensor, float(range)/1000);
190  vpDisplay::displayCharString(I, i-7, j+7, legend, vpColor::blue);
191  }
192 #endif
193  }
194 
195  }
196 
197 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
198  if (isInitialized)
199  vpDisplay::flush(I);
200 #endif
201 
202  fflush(stdout);
203 }
204 
210 int main(int argc, char **argv)
211 {
212  try {
213  ArArgumentParser parser(&argc, argv);
214  parser.loadDefaultArguments();
215 
216  robot = new vpRobotPioneer;
217 
218  // ArRobotConnector connects to the robot, get some initial data from it such as type and name,
219  // and then loads parameter files for this robot.
220  ArRobotConnector robotConnector(&parser, robot);
221  if(!robotConnector.connectRobot())
222  {
223  ArLog::log(ArLog::Terse, "Could not connect to the robot");
224  if(parser.checkHelpAndWarnUnparsed())
225  {
226  Aria::logOptions();
227  Aria::exit(1);
228  }
229  }
230  if (!Aria::parseArgs())
231  {
232  Aria::logOptions();
233  Aria::shutdown();
234  return false;
235  }
236 
237  std::cout << "Robot connected" << std::endl;
238 
239 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
240  // Create a display to show sensor data
241  if (isInitialized == false)
242  {
243  I.resize((unsigned int)half_size*2, (unsigned int)half_size*2);
244  I = 255;
245 
246 #if defined(VISP_HAVE_X11)
247  d = new vpDisplayX;
248 #elif defined (VISP_HAVE_GDI)
249  d = new vpDisplayGDI;
250 #endif
251  d->init(I, -1, -1, "Sonar range data");
252  isInitialized = true;
253  }
254 #endif
255 
256  // Activates the sonar
257  ArGlobalFunctor sonarPrinterCB(&sonarPrinter);
258  robot->addRangeDevice(&sonar);
259  robot->addUserTask("Sonar printer", 50, &sonarPrinterCB);
260 
261  robot->useSonar(true); // activates the sonar device usage
262 
263  // Robot velocities
264  vpColVector v_mes(2);
265 
266  for (int i=0; i < 1000; i++)
267  {
268  double t = vpTime::measureTimeMs();
269 
270  v_mes = robot->getVelocity(vpRobot::REFERENCE_FRAME);
271  std::cout << "Trans. vel= " << v_mes[0] << " m/s, Rot. vel=" << vpMath::deg(v_mes[1]) << " deg/s" << std::endl;
272  v_mes = robot->getVelocity(vpRobot::ARTICULAR_FRAME);
273  std::cout << "Left wheel vel= " << v_mes[0] << " m/s, Right wheel vel=" << v_mes[1] << " m/s" << std::endl;
274  std::cout << "Battery=" << robot->getBatteryVoltage() << std::endl;
275 
276 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
277  if (isInitialized) {
278  // A mouse click to exit
279  // Before exiting save the last sonar image
280  if (vpDisplay::getClick(I, false) == true) {
281  {
282  // Set the default output path
283  std::string opath;
284 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
285  opath = "/tmp";
286 #elif defined(_WIN32)
287  opath = "C:\\temp";
288 #endif
289  std::string username = vpIoTools::getUserName();
290 
291  // Append to the output path string, the login name of the user
292  opath = vpIoTools::createFilePath(opath, username);
293 
294  // Test if the output path exist. If no try to create it
295  if (vpIoTools::checkDirectory(opath) == false) {
296  try {
297  // Create the dirname
299  }
300  catch (...) {
301  std::cerr << std::endl
302  << "ERROR:" << std::endl;
303  std::cerr << " Cannot create " << opath << std::endl;
304  exit(-1);
305  }
306  }
307  std::string filename = opath + "/sonar.png";
308  vpImage<vpRGBa> C;
309  vpDisplay::getImage(I, C);
310  vpImageIo::write(C, filename);
311  }
312  break;
313  }
314  }
315 #endif
316 
317  vpTime::wait(t, 40);
318  }
319 
320  ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping.");
321  robot->lock();
322  robot->stop();
323  robot->unlock();
324  ArUtil::sleep(1000);
325 
326  robot->lock();
327  ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
328  robot->getX(), robot->getY(), robot->getTh(), robot->getVel(), robot->getRotVel(), robot->getBatteryVoltage());
329  robot->unlock();
330 
331  std::cout << "Ending robot thread..." << std::endl;
332  robot->stopRunning();
333 
334 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
335  if (isInitialized) {
336  if (d != NULL)
337  delete d;
338  }
339 #endif
340 
341  // wait for the thread to stop
342  robot->waitForRunExit();
343 
344  delete robot;
345 
346  // exit
347  ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting.");
348  return 0;
349  }
350  catch(vpException &e) {
351  std::cout << "Catch an exception: " << e << std::endl;
352  return 1;
353  }
354 }
355 
356 #endif
357 
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:157
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:358
void useSonar(bool usage)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:153
error that can be emited by ViSP classes.
Definition: vpException.h:73
Interface for Pioneer mobile robots based on Aria 3rd party library.
static const vpColor green
Definition: vpColor.h:166
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
static const vpColor red
Definition: vpColor.h:163
static void write(const vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:368
static void makeDirectory(const char *dirname)
Definition: vpIoTools.cpp:427
static std::string createFilePath(const std::string &parent, const std::string child)
Definition: vpIoTools.cpp:1366
static void display(const vpImage< unsigned char > &I)
static std::string getUserName()
Definition: vpIoTools.cpp:177
void resize(const unsigned int h, const unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:903
static void getImage(const vpImage< unsigned char > &Is, vpImage< vpRGBa > &Id)
Definition: vpDisplay.cpp:155
static double rad(double deg)
Definition: vpMath.h:104
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static double deg(double rad)
Definition: vpMath.h:97
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
static void displayCharString(const vpImage< unsigned char > &I, const vpImagePoint &ip, const char *string, const vpColor &color)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
static const vpColor blue
Definition: vpColor.h:169