41 #include <visp3/robot/vpRobotPioneer.h>
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>
51 #ifndef VISP_HAVE_PIONEER
54 std::cout <<
"\nThis example requires Aria 3rd party library. You should "
64 #if defined(VISP_HAVE_X11)
66 #elif defined(VISP_HAVE_GDI)
70 static bool isInitialized =
false;
71 static int half_size = 256 * 2;
73 void sonarPrinter(
void)
75 fprintf(stdout,
"in sonarPrinter()\n");
77 double scale = (double)half_size / (
double)sonar.getMaxRange();
108 printf(
"Closest readings within polar sections:\n");
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);
115 if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
116 printf(
"%3.0f ", angle);
118 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
120 if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon()) {
126 double j = -y * scale + half_size;
127 double i = -x * scale + half_size;
137 range = sonar.currentReadingPolar(-135, -45, &angle);
138 printf(
" right quadrant: %5.0f ", range);
140 if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
141 printf(
"%3.0f ", angle);
144 range = sonar.currentReadingPolar(45, 135, &angle);
145 printf(
" left quadrant: %5.0f ", range);
147 if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
148 printf(
"%3.0f ", angle);
151 range = sonar.currentReadingPolar(-135, 135, &angle);
152 printf(
" back quadrant: %5.0f ", range);
154 if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
155 printf(
"%3.0f ", angle);
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();
168 double sy = reading->getSensorY();
176 double sj = -sy * scale + half_size;
177 double si = -sx * scale + half_size;
178 double j = -y * scale + half_size;
179 double i = -x * scale + half_size;
186 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
188 if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon()) {
192 sprintf(legend,
"%d: %1.2fm", sensor,
float(range) / 1000);
199 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
212 int main(
int argc,
char **argv)
215 ArArgumentParser parser(&argc, argv);
216 parser.loadDefaultArguments();
222 ArRobotConnector robotConnector(&parser, robot);
223 if (!robotConnector.connectRobot()) {
224 ArLog::log(ArLog::Terse,
"Could not connect to the robot");
225 if (parser.checkHelpAndWarnUnparsed()) {
230 if (!Aria::parseArgs()) {
236 std::cout <<
"Robot connected" << std::endl;
238 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
240 if (isInitialized ==
false) {
241 I.
resize((
unsigned int)half_size * 2, (
unsigned int)half_size * 2);
244 #if defined(VISP_HAVE_X11)
246 #elif defined(VISP_HAVE_GDI)
249 d->
init(I, -1, -1,
"Sonar range data");
250 isInitialized =
true;
255 ArGlobalFunctor sonarPrinterCB(&sonarPrinter);
256 robot->addRangeDevice(&sonar);
257 robot->addUserTask(
"Sonar printer", 50, &sonarPrinterCB);
259 robot->useSonar(
true);
264 for (
int i = 0; i < 1000; i++) {
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;
273 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
281 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
283 #elif defined(_WIN32)
297 std::cerr << std::endl <<
"ERROR:" << std::endl;
298 std::cerr <<
" Cannot create " << opath << std::endl;
302 std::string filename = opath +
"/sonar.png";
315 ArLog::log(ArLog::Normal,
"simpleMotionCommands: Stopping.");
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());
329 std::cout <<
"Ending robot thread..." << std::endl;
330 robot->stopRunning();
332 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
340 robot->waitForRunExit();
345 ArLog::log(ArLog::Normal,
"simpleMotionCommands: Exiting.");
348 std::cout <<
"Catch an exception: " << e << std::endl;