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 bool 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;
VISP_EXPORT int wait(double t0, double t)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void useSonar(bool usage)
Display for windows using GDI (available on any windows 32 platform).
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
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)
VISP_EXPORT double measureTimeMs()
static void write(const vpImage< unsigned char > &I, const std::string &filename)
static void display(const vpImage< unsigned char > &I)
void resize(const unsigned int h, const unsigned int w)
resize the image : Image initialization
static void getImage(const vpImage< unsigned char > &Is, vpImage< vpRGBa > &Id)
static double rad(double deg)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static double deg(double rad)
Implementation of column vector and the associated operations.
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
static void displayCharString(const vpImage< unsigned char > &I, const vpImagePoint &ip, const char *string, const vpColor &color)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
static const vpColor blue