Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
sonarPioneerReader.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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*****************************************************************************/
35
36#include <iostream>
37
38#include <visp3/core/vpConfig.h>
39#include <visp3/core/vpDisplay.h>
40#include <visp3/core/vpImage.h>
41#include <visp3/core/vpIoTools.h>
42#include <visp3/core/vpTime.h>
43#include <visp3/gui/vpDisplayGDI.h>
44#include <visp3/gui/vpDisplayX.h>
45#include <visp3/io/vpImageIo.h>
46#include <visp3/robot/vpRobotPioneer.h> // Include first to avoid build issues with Status, None, isfinite
47
48#ifndef VISP_HAVE_PIONEER
49int main()
50{
51 std::cout << "\nThis example requires Aria 3rd party library. You should "
52 "install it.\n"
53 << std::endl;
54 return EXIT_SUCCESS;
55}
56
57#else
58
59ArSonarDevice sonar;
60vpRobotPioneer *robot;
61#if defined(VISP_HAVE_X11)
62vpDisplayX *d;
63#elif defined(VISP_HAVE_GDI)
65#endif
67static bool isInitialized = false;
68static int half_size = 256 * 2;
69
70void sonarPrinter(void)
71{
72 fprintf(stdout, "in sonarPrinter()\n");
73 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 double x = range * cos(vpMath::rad(angle)); // position of the obstacle in the sensor frame
119 double y = range * sin(vpMath::rad(angle));
120
121 // Conversion in pixels so that the robot frame is in the middle of the
122 // 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 reading = robot->getSonarReading(sensor);
161 if (reading != NULL) {
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
172 // image
173 double sj = -sy * scale + half_size; // sensor position
174 double si = -sx * scale + half_size;
175 double j = -y * scale + half_size; // obstacle position
176 double i = -x * scale + half_size;
177
178 // printf("%d x: %.1f y: %.1f th: %.1f d: %d\n", sensor,
179 // reading->getSensorX(),
180 // reading->getSensorY(), reading->getSensorTh(),
181 // reading->getRange());
182
183#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
184 // if (isInitialized && range != sonar.getMaxRange())
185 if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon()) {
186 vpDisplay::displayLine(I, si, sj, i, j, vpColor::blue, 2);
188 std::stringstream legend;
189 legend << sensor << ": " << std::setprecision(2) << float(range) / 1000.;
190 vpDisplay::displayText(I, i - 7, j + 7, legend.str(), vpColor::blue);
191 }
192#endif
193 }
194 }
195
196#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
197 if (isInitialized)
199#endif
200
201 fflush(stdout);
202}
203
209int main(int argc, char **argv)
210{
211 try {
212 ArArgumentParser parser(&argc, argv);
213 parser.loadDefaultArguments();
214
215 robot = new vpRobotPioneer;
216
217 // ArRobotConnector connects to the robot, get some initial data from it
218 // such as type and name, and then loads parameter files for this robot.
219 ArRobotConnector robotConnector(&parser, robot);
220 if (!robotConnector.connectRobot()) {
221 ArLog::log(ArLog::Terse, "Could not connect to the robot");
222 if (parser.checkHelpAndWarnUnparsed()) {
223 Aria::logOptions();
224 Aria::exit(1);
225 }
226 }
227 if (!Aria::parseArgs()) {
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 I.resize((unsigned int)half_size * 2, (unsigned int)half_size * 2);
239 I = 255;
240
241#if defined(VISP_HAVE_X11)
242 d = new vpDisplayX;
243#elif defined(VISP_HAVE_GDI)
244 d = new vpDisplayGDI;
245#endif
246 d->init(I, -1, -1, "Sonar range data");
247 isInitialized = true;
248 }
249#endif
250
251 // Activates the sonar
252 ArGlobalFunctor sonarPrinterCB(&sonarPrinter);
253 robot->addRangeDevice(&sonar);
254 robot->addUserTask("Sonar printer", 50, &sonarPrinterCB);
255
256 robot->useSonar(true); // activates the sonar device usage
257
258 // Robot velocities
259 vpColVector v_mes(2);
260
261 for (int i = 0; i < 1000; i++) {
262 double t = vpTime::measureTimeMs();
263
265 std::cout << "Trans. vel= " << v_mes[0] << " m/s, Rot. vel=" << vpMath::deg(v_mes[1]) << " deg/s" << std::endl;
267 std::cout << "Left wheel vel= " << v_mes[0] << " m/s, Right wheel vel=" << v_mes[1] << " m/s" << std::endl;
268 std::cout << "Battery=" << robot->getBatteryVoltage() << std::endl;
269
270#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
271 if (isInitialized) {
272 // A mouse click to exit
273 // Before exiting save the last sonar image
274 if (vpDisplay::getClick(I, false) == true) {
275 {
276 // Set the default output path
277 std::string opath;
278#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
279 opath = "/tmp";
280#elif defined(_WIN32)
281 opath = "C:\\temp";
282#endif
283 std::string username = vpIoTools::getUserName();
284
285 // Append to the output path string, the login name of the user
286 opath = vpIoTools::createFilePath(opath, username);
287
288 // Test if the output path exist. If no try to create it
289 if (vpIoTools::checkDirectory(opath) == false) {
290 try {
291 // Create the dirname
293 } catch (...) {
294 std::cerr << std::endl << "ERROR:" << std::endl;
295 std::cerr << " Cannot create " << opath << std::endl;
296 return EXIT_FAILURE;
297 }
298 }
299 std::string filename = opath + "/sonar.png";
302 vpImageIo::write(C, filename);
303 }
304 break;
305 }
306 }
307#endif
308
309 vpTime::wait(t, 40);
310 }
311
312 ArLog::log(ArLog::Normal, "simpleMotionCommands: Stopping.");
313 robot->lock();
314 robot->stop();
315 robot->unlock();
316 ArUtil::sleep(1000);
317
318 robot->lock();
319 ArLog::log(ArLog::Normal,
320 "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. "
321 "Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
322 robot->getX(), robot->getY(), robot->getTh(), robot->getVel(), robot->getRotVel(),
323 robot->getBatteryVoltage());
324 robot->unlock();
325
326 std::cout << "Ending robot thread..." << std::endl;
327 robot->stopRunning();
328
329#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
330 if (isInitialized) {
331 if (d != NULL)
332 delete d;
333 }
334#endif
335
336 // wait for the thread to stop
337 robot->waitForRunExit();
338
339 delete robot;
340
341 // exit
342 ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting.");
343 return EXIT_SUCCESS;
344 } catch (const vpException &e) {
345 std::cout << "Catch an exception: " << e << std::endl;
346 return EXIT_FAILURE;
347 }
348}
349
350#endif
Implementation of column vector and the associated operations.
static const vpColor red
Definition vpColor.h:211
static const vpColor blue
Definition vpColor.h:217
static const vpColor green
Definition vpColor.h:214
Display for windows using GDI (available on any windows 32 platform).
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
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)
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 emitted by ViSP classes.
Definition vpException.h:59
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition of the vpImage class member functions.
Definition vpImage.h:135
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition vpImage.h:795
static bool checkDirectory(const std::string &dirname)
static std::string getUserName()
static std::string createFilePath(const std::string &parent, const std::string &child)
static void makeDirectory(const std::string &dirname)
static double rad(double deg)
Definition vpMath.h:116
static double deg(double rad)
Definition vpMath.h:106
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:74
@ ARTICULAR_FRAME
Definition vpRobot.h:76
VISP_EXPORT double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)