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