Visual Servoing Platform version 3.5.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
52int 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
62ArSonarDevice sonar;
63vpRobotPioneer *robot;
64#if defined(VISP_HAVE_X11)
65vpDisplayX *d;
66#elif defined(VISP_HAVE_GDI)
68#endif
70static bool isInitialized = false;
71static int half_size = 256 * 2;
72
73void 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);
191 std::stringstream legend;
192 legend << sensor << ": " << std::setprecision( 2 ) << float( range ) / 1000.;
193 vpDisplay::displayText( I, i - 7, j + 7, legend.str(), vpColor::blue );
194 }
195#endif
196 }
197 }
198
199#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
200 if (isInitialized)
202#endif
203
204 fflush(stdout);
205}
206
212int 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
268 std::cout << "Trans. vel= " << v_mes[0] << " m/s, Rot. vel=" << vpMath::deg(v_mes[1]) << " deg/s" << std::endl;
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";
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
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static const vpColor red
Definition: vpColor.h:217
static const vpColor blue
Definition: vpColor.h:223
static const vpColor green
Definition: vpColor.h:220
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
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 void getImage(const vpImage< unsigned char > &Is, vpImage< vpRGBa > &Id)
Definition: vpDisplay.cpp:144
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
Definition: vpException.h:72
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:293
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:800
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:420
static std::string getUserName()
Definition: vpIoTools.cpp:316
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1670
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:570
static double rad(double deg)
Definition: vpMath.h:110
static double deg(double rad)
Definition: vpMath.h:103
Interface for Pioneer mobile robots based on Aria 3rd party library.
void useSonar(bool usage)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()