40 #include <visp3/robot/vpRobotPioneer.h> 41 #include <visp3/core/vpConfig.h> 42 #include <visp3/core/vpDisplay.h> 43 #include <visp3/gui/vpDisplayGDI.h> 44 #include <visp3/gui/vpDisplayX.h> 45 #include <visp3/core/vpImage.h> 46 #include <visp3/core/vpIoTools.h> 47 #include <visp3/io/vpImageIo.h> 48 #include <visp3/core/vpTime.h> 50 #ifndef VISP_HAVE_PIONEER 53 std::cout <<
"\nThis example requires Aria 3rd party library. You should install it.\n" 62 #if defined(VISP_HAVE_X11) 64 #elif defined (VISP_HAVE_GDI) 68 static int isInitialized =
false;
69 static int half_size = 256*2;
71 void sonarPrinter(
void)
73 fprintf(stdout,
"in sonarPrinter()\n"); fflush(stdout);
74 double scale = (double)half_size / (
double)sonar.getMaxRange();
105 printf(
"Closest readings within polar sections:\n");
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);
112 if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
113 printf(
"%3.0f ", angle);
115 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI) 117 if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
123 double j = -y * scale + half_size;
124 double i = -x * scale + half_size;
134 range = sonar.currentReadingPolar(-135, -45, &angle);
135 printf(
" right quadrant: %5.0f ", range);
137 if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
138 printf(
"%3.0f ", angle);
141 range = sonar.currentReadingPolar(45, 135, &angle);
142 printf(
" left quadrant: %5.0f ", range);
144 if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
145 printf(
"%3.0f ", angle);
148 range = sonar.currentReadingPolar(-135, 135, &angle);
149 printf(
" back quadrant: %5.0f ", range);
151 if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
152 printf(
"%3.0f ", angle);
158 ArSensorReading *reading;
159 for (
int sensor = 0; sensor < robot->getNumSonar(); sensor++)
161 reading = robot->getSonarReading(sensor);
164 angle = reading->getSensorTh();
165 range = reading->getRange();
166 double sx = reading->getSensorX();
167 double sy = reading->getSensorY();
174 double sj = -sy * scale + half_size;
175 double si = -sx * scale + half_size;
176 double j = -y * scale + half_size;
177 double i = -x * scale + half_size;
182 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI) 184 if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
189 sprintf(legend,
"%d: %1.2fm", sensor,
float(range)/1000);
197 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI) 210 int main(
int argc,
char **argv)
213 ArArgumentParser parser(&argc, argv);
214 parser.loadDefaultArguments();
220 ArRobotConnector robotConnector(&parser, robot);
221 if(!robotConnector.connectRobot())
223 ArLog::log(ArLog::Terse,
"Could not connect to the robot");
224 if(parser.checkHelpAndWarnUnparsed())
230 if (!Aria::parseArgs())
237 std::cout <<
"Robot connected" << std::endl;
239 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI) 241 if (isInitialized ==
false)
243 I.
resize((
unsigned int)half_size*2, (
unsigned int)half_size*2);
246 #if defined(VISP_HAVE_X11) 248 #elif defined (VISP_HAVE_GDI) 251 d->
init(I, -1, -1,
"Sonar range data");
252 isInitialized =
true;
257 ArGlobalFunctor sonarPrinterCB(&sonarPrinter);
258 robot->addRangeDevice(&sonar);
259 robot->addUserTask(
"Sonar printer", 50, &sonarPrinterCB);
266 for (
int i=0; i < 1000; i++)
271 std::cout <<
"Trans. vel= " << v_mes[0] <<
" m/s, Rot. vel=" <<
vpMath::deg(v_mes[1]) <<
" deg/s" << std::endl;
273 std::cout <<
"Left wheel vel= " << v_mes[0] <<
" m/s, Right wheel vel=" << v_mes[1] <<
" m/s" << std::endl;
274 std::cout <<
"Battery=" << robot->getBatteryVoltage() << std::endl;
276 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI) 284 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX 286 #elif defined(_WIN32) 301 std::cerr << std::endl
302 <<
"ERROR:" << std::endl;
303 std::cerr <<
" Cannot create " << opath << std::endl;
307 std::string filename = opath +
"/sonar.png";
320 ArLog::log(ArLog::Normal,
"simpleMotionCommands: Stopping.");
327 ArLog::log(ArLog::Normal,
"simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
328 robot->getX(), robot->getY(), robot->getTh(), robot->getVel(), robot->getRotVel(), robot->getBatteryVoltage());
331 std::cout <<
"Ending robot thread..." << std::endl;
332 robot->stopRunning();
334 #if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI) 342 robot->waitForRunExit();
347 ArLog::log(ArLog::Normal,
"simpleMotionCommands: Exiting.");
351 std::cout <<
"Catch an exception: " << e << std::endl;
static void write(const vpImage< unsigned char > &I, const char *filename)
VISP_EXPORT int wait(double t0, double t)
void useSonar(bool usage)
Display for windows using GDI (available on any windows 32 platform).
Define the X11 console to display images.
error that can be emited by ViSP classes.
Interface for Pioneer mobile robots based on Aria 3rd party library.
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 resize(const unsigned int h, const unsigned int w)
set the size of the image without initializing it.
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)
VISP_EXPORT double measureTimeMs()
static double deg(double rad)
Implementation of column vector and the associated operations.
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