44 #include <visp/vpRobotPioneer.h>
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/vpIoTools.h>
51 #include <visp/vpImageIo.h>
52 #include <visp/vpTime.h>
54 #ifndef VISP_HAVE_PIONEER
57 std::cout <<
"\nThis example requires Aria 3rd party library. You should install it.\n"
66 #if defined(VISP_HAVE_X11)
68 #elif defined (VISP_HAVE_GDI)
72 static int isInitialized =
false;
73 static int half_size = 256*2;
75 void sonarPrinter(
void)
77 fprintf(stdout,
"in sonarPrinter()\n"); fflush(stdout);
78 double scale = (double)half_size / (
double)sonar.getMaxRange();
109 printf(
"Closest readings within polar sections:\n");
111 double start_angle = -45;
112 double end_angle = 45;
113 range = sonar.currentReadingPolar(start_angle, end_angle, &angle);
114 printf(
" front quadrant: %5.0f ", range);
115 if (range != sonar.getMaxRange())
116 printf(
"%3.0f ", angle);
118 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
119 if (isInitialized && range != sonar.getMaxRange())
125 double j = -y * scale + half_size;
126 double i = -x * scale + half_size;
136 range = sonar.currentReadingPolar(-135, -45, &angle);
137 printf(
" right quadrant: %5.0f ", range);
138 if (range != sonar.getMaxRange())
139 printf(
"%3.0f ", angle);
142 range = sonar.currentReadingPolar(45, 135, &angle);
143 printf(
" left quadrant: %5.0f ", range);
144 if (range != sonar.getMaxRange())
145 printf(
"%3.0f ", angle);
148 range = sonar.currentReadingPolar(-135, 135, &angle);
149 printf(
" back quadrant: %5.0f ", range);
150 if (range != sonar.getMaxRange())
151 printf(
"%3.0f ", angle);
157 ArSensorReading *reading;
158 for (
int sensor = 0; sensor < robot->getNumSonar(); sensor++)
160 reading = robot->getSonarReading(sensor);
163 angle = reading->getSensorTh();
164 range = reading->getRange();
165 double sx = reading->getSensorX();
166 double sy = reading->getSensorY();
173 double sj = -sy * scale + half_size;
174 double si = -sx * scale + half_size;
175 double j = -y * scale + half_size;
176 double i = -x * scale + half_size;
181 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
182 if (isInitialized && range != sonar.getMaxRange())
187 sprintf(legend,
"%d: %1.2fm", sensor,
float(range)/1000);
195 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
208 int main(
int argc,
char **argv)
210 ArArgumentParser parser(&argc, argv);
211 parser.loadDefaultArguments();
217 ArRobotConnector robotConnector(&parser, robot);
218 if(!robotConnector.connectRobot())
220 ArLog::log(ArLog::Terse,
"Could not connect to the robot");
221 if(parser.checkHelpAndWarnUnparsed())
227 if (!Aria::parseArgs())
234 std::cout <<
"Robot connected" << std::endl;
236 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
238 if (isInitialized ==
false)
240 I.
resize(half_size*2, half_size*2);
243 #if defined(VISP_HAVE_X11)
245 #elif defined (VISP_HAVE_GDI)
248 d->
init(I, -1, -1,
"Sonar range data");
249 isInitialized =
true;
254 ArGlobalFunctor sonarPrinterCB(&sonarPrinter);
255 robot->addRangeDevice(&sonar);
256 robot->addUserTask(
"Sonar printer", 50, &sonarPrinterCB);
263 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)
298 std::cerr << std::endl
299 <<
"ERROR:" << std::endl;
300 std::cerr <<
" Cannot create " << opath << std::endl;
304 std::string filename = opath +
"/sonar.png";
317 ArLog::log(ArLog::Normal,
"simpleMotionCommands: Stopping.");
324 ArLog::log(ArLog::Normal,
"simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
325 robot->getX(), robot->getY(), robot->getTh(), robot->getVel(), robot->getRotVel(), robot->getBatteryVoltage());
328 std::cout <<
"Ending robot thread..." << std::endl;
329 robot->stopRunning();
331 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
339 robot->waitForRunExit();
344 ArLog::log(ArLog::Normal,
"simpleMotionCommands: Exiting.");