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/vpImageIo.h>
51 #include <visp/vpTime.h>
53 #ifndef VISP_HAVE_PIONEER
56 std::cout <<
"\nThis example requires Aria 3rd party library. You should install it.\n"
65 #if defined(VISP_HAVE_X11)
67 #elif defined (VISP_HAVE_GDI)
71 static int isInitialized =
false;
72 static int half_size = 256*2;
74 void sonarPrinter(
void)
76 fprintf(stdout,
"in sonarPrinter()\n"); fflush(stdout);
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);
114 if (range != sonar.getMaxRange())
115 printf(
"%3.0f ", angle);
117 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
118 if (isInitialized && range != sonar.getMaxRange())
124 double j = -y * scale + half_size;
125 double i = -x * scale + half_size;
135 range = sonar.currentReadingPolar(-135, -45, &angle);
136 printf(
" right quadrant: %5.0f ", range);
137 if (range != sonar.getMaxRange())
138 printf(
"%3.0f ", angle);
141 range = sonar.currentReadingPolar(45, 135, &angle);
142 printf(
" left quadrant: %5.0f ", range);
143 if (range != sonar.getMaxRange())
144 printf(
"%3.0f ", angle);
147 range = sonar.currentReadingPolar(-135, 135, &angle);
148 printf(
" back quadrant: %5.0f ", range);
149 if (range != sonar.getMaxRange())
150 printf(
"%3.0f ", angle);
156 ArSensorReading *reading;
157 for (
int sensor = 0; sensor < robot->getNumSonar(); sensor++)
159 reading = robot->getSonarReading(sensor);
162 angle = reading->getSensorTh();
163 range = reading->getRange();
164 double sx = reading->getSensorX();
165 double sy = reading->getSensorY();
172 double sj = -sy * scale + half_size;
173 double si = -sx * scale + half_size;
174 double j = -y * scale + half_size;
175 double i = -x * scale + half_size;
180 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
181 if (isInitialized && range != sonar.getMaxRange())
186 sprintf(legend,
"%d: %1.2fm", sensor,
float(range)/1000);
194 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
207 int main(
int argc,
char **argv)
209 ArArgumentParser parser(&argc, argv);
210 parser.loadDefaultArguments();
216 ArRobotConnector robotConnector(&parser, robot);
217 if(!robotConnector.connectRobot())
219 ArLog::log(ArLog::Terse,
"Could not connect to the robot");
220 if(parser.checkHelpAndWarnUnparsed())
226 if (!Aria::parseArgs())
233 std::cout <<
"Robot connected" << std::endl;
235 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
237 if (isInitialized ==
false)
239 I.
resize(half_size*2, half_size*2);
242 #if defined(VISP_HAVE_X11)
244 #elif defined (VISP_HAVE_GDI)
247 d->
init(I, -1, -1,
"Sonar range data");
248 isInitialized =
true;
253 ArGlobalFunctor sonarPrinterCB(&sonarPrinter);
254 robot->addRangeDevice(&sonar);
255 robot->addUserTask(
"Sonar printer", 50, &sonarPrinterCB);
262 for (
int i=0; i < 1000; i++)
267 std::cout <<
"Trans. vel= " << v_mes[0] <<
" m/s, Rot. vel=" <<
vpMath::deg(v_mes[1]) <<
" deg/s" << std::endl;
269 std::cout <<
"Left wheel vel= " << v_mes[0] <<
" m/s, Right wheel vel=" << v_mes[1] <<
" m/s" << std::endl;
270 std::cout <<
"Battery=" << robot->getBatteryVoltage() << std::endl;
272 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
289 ArLog::log(ArLog::Normal,
"simpleMotionCommands: Stopping.");
296 ArLog::log(ArLog::Normal,
"simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
297 robot->getX(), robot->getY(), robot->getTh(), robot->getVel(), robot->getRotVel(), robot->getBatteryVoltage());
300 std::cout <<
"Ending robot thread..." << std::endl;
301 robot->stopRunning();
303 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
311 robot->waitForRunExit();
316 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