ViSP  2.6.2
sonarPioneerReader.cpp
1 /****************************************************************************
2  *
3  * $Id: sonarPioneerReader.cpp 3820 2012-06-27 13:13:29Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 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  * Example that shows how to control a Pioneer mobile robot in ViSP.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
42 #include <iostream>
43 
44 #include <visp/vpRobotPioneer.h> // Include before vpDisplayX to avoid build issues
45 #include <visp/vpConfig.h>
46 #include <visp/vpDisplay.h>
47 #include <visp/vpDisplayGDI.h>
48 #include <visp/vpDisplayX.h>
49 #include <visp/vpImage.h>
50 #include <visp/vpImageIo.h>
51 #include <visp/vpTime.h>
52 
53 #ifndef VISP_HAVE_PIONEER
54 int main()
55 {
56  std::cout << "\nThis example requires Aria 3rd party library. You should install it.\n"
57  << std::endl;
58  return 0;
59 }
60 
61 #else
62 
63 ArSonarDevice sonar;
64 vpRobotPioneer *robot;
65 #if defined(VISP_HAVE_X11)
66 vpDisplayX *d;
67 #elif defined (VISP_HAVE_GDI)
68 vpDisplayGDI *d;
69 #endif
71 static int isInitialized = false;
72 static int half_size = 256*2;
73 
74 void sonarPrinter(void)
75 {
76  fprintf(stdout, "in sonarPrinter()\n"); 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  printf("%3.0f ", angle);
116  printf("\n");
117 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
118  if (isInitialized && range != sonar.getMaxRange())
119  {
120  double x = range * cos(vpMath::rad(angle)); // position of the obstacle in the sensor frame
121  double y = range * sin(vpMath::rad(angle));
122 
123  // Conversion in pixels so that the robot frame is in the middle of the image
124  double j = -y * scale + half_size; // obstacle position
125  double i = -x * scale + half_size;
126 
128  vpDisplay::displayLine(I, half_size, half_size, 0, 0, vpColor::red, 5);
129  vpDisplay::displayLine(I, half_size, half_size, 0, 2*half_size-1, vpColor::red, 5);
130  vpDisplay::displayLine(I, half_size, half_size, i, j, vpColor::green, 3);
132  }
133 #endif
134 
135  range = sonar.currentReadingPolar(-135, -45, &angle);
136  printf(" right quadrant: %5.0f ", range);
137  if (range != sonar.getMaxRange())
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  printf("%3.0f ", angle);
145  printf("\n");
146 
147  range = sonar.currentReadingPolar(-135, 135, &angle);
148  printf(" back quadrant: %5.0f ", range);
149  if (range != sonar.getMaxRange())
150  printf("%3.0f ", angle);
151  printf("\n");
152 
153  /*
154  * example to show how get all sonar sensor data
155  */
156  ArSensorReading *reading;
157  for (int sensor = 0; sensor < robot->getNumSonar(); sensor++)
158  {
159  reading = robot->getSonarReading(sensor);
160  if (reading != NULL)
161  {
162  angle = reading->getSensorTh();
163  range = reading->getRange();
164  double sx = reading->getSensorX(); // position of the sensor in the robot frame
165  double sy = reading->getSensorY();
166  double ox = range * cos(vpMath::rad(angle)); // position of the obstacle in the sensor frame
167  double oy = range * sin(vpMath::rad(angle));
168  double x = sx + ox; // position of the obstacle in the robot frame
169  double y = sy + oy;
170 
171  // Conversion in pixels so that the robot frame is in the middle of the image
172  double sj = -sy * scale + half_size; // sensor position
173  double si = -sx * scale + half_size;
174  double j = -y * scale + half_size; // obstacle position
175  double i = -x * scale + half_size;
176 
177 // printf("%d x: %.1f y: %.1f th: %.1f d: %d\n", sensor, reading->getSensorX(),
178 // reading->getSensorY(), reading->getSensorTh(), reading->getRange());
179 
180 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
181  if (isInitialized && range != sonar.getMaxRange())
182  {
183  vpDisplay::displayLine(I, si, sj, i, j, vpColor::blue, 2);
184  vpDisplay::displayCross(I, si, sj, 7, vpColor::blue);
185  char legend[15];
186  sprintf(legend, "%d: %1.2fm", sensor, float(range)/1000);
187  vpDisplay::displayCharString(I, i-7, j+7, legend, vpColor::blue);
188  }
189 #endif
190  }
191 
192  }
193 
194 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
195  if (isInitialized)
196  vpDisplay::flush(I);
197 #endif
198 
199  fflush(stdout);
200 }
201 
207 int main(int argc, char **argv)
208 {
209  ArArgumentParser parser(&argc, argv);
210  parser.loadDefaultArguments();
211 
212  robot = new vpRobotPioneer;
213 
214  // ArRobotConnector connects to the robot, get some initial data from it such as type and name,
215  // and then loads parameter files for this robot.
216  ArRobotConnector robotConnector(&parser, robot);
217  if(!robotConnector.connectRobot())
218  {
219  ArLog::log(ArLog::Terse, "Could not connect to the robot");
220  if(parser.checkHelpAndWarnUnparsed())
221  {
222  Aria::logOptions();
223  Aria::exit(1);
224  }
225  }
226  if (!Aria::parseArgs())
227  {
228  Aria::logOptions();
229  Aria::shutdown();
230  return false;
231  }
232 
233  std::cout << "Robot connected" << std::endl;
234 
235 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
236  // Create a display to show sensor data
237  if (isInitialized == false)
238  {
239  I.resize(half_size*2, half_size*2);
240  I = 255;
241 
242 #if defined(VISP_HAVE_X11)
243  d = new vpDisplayX;
244 #elif defined (VISP_HAVE_GDI)
245  d = new vpDisplayGDI;
246 #endif
247  d->init(I, -1, -1, "Sonar range data");
248  isInitialized = true;
249  }
250 #endif
251 
252  // Activates the sonar
253  ArGlobalFunctor sonarPrinterCB(&sonarPrinter);
254  robot->addRangeDevice(&sonar);
255  robot->addUserTask("Sonar printer", 50, &sonarPrinterCB);
256 
257  robot->useSonar(true); // activates the sonar device usage
258 
259  // Robot velocities
260  vpColVector v_mes(2);
261 
262  for (int i=0; i < 1000; i++)
263  {
264  double t = vpTime::measureTimeMs();
265 
266  v_mes = robot->getVelocity(vpRobot::REFERENCE_FRAME);
267  std::cout << "Trans. vel= " << v_mes[0] << " m/s, Rot. vel=" << vpMath::deg(v_mes[1]) << " deg/s" << std::endl;
268  v_mes = robot->getVelocity(vpRobot::ARTICULAR_FRAME);
269  std::cout << "Left wheel vel= " << v_mes[0] << " m/s, Right wheel vel=" << v_mes[1] << " m/s" << std::endl;
270  std::cout << "Battery=" << robot->getBatteryVoltage() << std::endl;
271 
272 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
273  if (isInitialized) {
274  // A mouse click to exit
275  if (vpDisplay::getClick(I, false) == true) {
276  {
277  vpImage<vpRGBa> C;
278  vpDisplay::getImage(I, C);
279  vpImageIo::write(C, "/tmp/sonar.png");
280  }
281  break;
282  }
283  }
284 #endif
285 
286  vpTime::wait(t, 40);
287  }
288 
289  ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping.");
290  robot->lock();
291  robot->stop();
292  robot->unlock();
293  ArUtil::sleep(1000);
294 
295  robot->lock();
296  ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
297  robot->getX(), robot->getY(), robot->getTh(), robot->getVel(), robot->getRotVel(), robot->getBatteryVoltage());
298  robot->unlock();
299 
300  std::cout << "Ending robot thread..." << std::endl;
301  robot->stopRunning();
302 
303 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
304  if (isInitialized) {
305  if (d != NULL)
306  delete d;
307  }
308 #endif
309 
310  // wait for the thread to stop
311  robot->waitForRunExit();
312 
313  delete robot;
314 
315  // exit
316  ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting.");
317  return 0;
318 }
319 
320 #endif
321 
static void write(const vpImage< unsigned char > &I, const char *filename)
Definition: vpImageIo.cpp:355
void useSonar(bool usage)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
void resize(const unsigned int height, const unsigned int width)
set the size of the image
Definition: vpImage.h:530
Define the X11 console to display images.
Definition: vpDisplayX.h:152
Interface for Pioneer mobile robots based on Aria 3rd party library.
static double measureTimeMs()
Definition: vpTime.cpp:86
static int wait(double t0, double t)
Definition: vpTime.cpp:149
static const vpColor green
Definition: vpColor.h:168
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:1964
static const vpColor red
Definition: vpColor.h:165
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:186
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
static void getImage(const vpImage< unsigned char > &Is, vpImage< vpRGBa > &Id)
Definition: vpDisplay.cpp:300
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
virtual void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color=vpColor::green)=0
virtual bool getClick(bool blocking=true)=0
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
static const vpColor blue
Definition: vpColor.h:171