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.");
static void write(const vpImage< unsigned char > &I, const char *filename)
void useSonar(bool usage)
Display for windows using GDI (available on any windows 32 platform).
void resize(const unsigned int height, const unsigned int width)
set the size of the image
Define the X11 console to display images.
Interface for Pioneer mobile robots based on Aria 3rd party library.
static double measureTimeMs()
static int wait(double t0, double t)
static const vpColor green
static void flush(const vpImage< unsigned char > &I)
static void display(const vpImage< unsigned char > &I)
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)
static double rad(double deg)
static double deg(double rad)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
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