38 #include <visp3/core/vpConfig.h>
39 #include <visp3/core/vpDisplay.h>
40 #include <visp3/core/vpImage.h>
41 #include <visp3/core/vpIoTools.h>
42 #include <visp3/core/vpTime.h>
43 #include <visp3/gui/vpDisplayGDI.h>
44 #include <visp3/gui/vpDisplayX.h>
45 #include <visp3/io/vpImageIo.h>
46 #include <visp3/robot/vpRobotPioneer.h>
48 #ifndef VISP_HAVE_PIONEER
51 std::cout <<
"\nThis example requires Aria 3rd party library. You should "
61 #if defined(VISP_HAVE_X11)
63 #elif defined(VISP_HAVE_GDI)
67 static bool isInitialized =
false;
68 static int half_size = 256 * 2;
70 void sonarPrinter(
void)
72 fprintf(stdout,
"in sonarPrinter()\n");
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++) {
160 reading = robot->getSonarReading(sensor);
161 if (reading !=
nullptr) {
162 angle = reading->getSensorTh();
163 range = reading->getRange();
164 double sx = reading->getSensorX();
165 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;
183 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
185 if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon()) {
188 std::stringstream legend;
189 legend << sensor <<
": " << std::setprecision(2) << float(range) / 1000.;
196 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
209 int main(
int argc,
char **argv)
212 ArArgumentParser parser(&argc, argv);
213 parser.loadDefaultArguments();
219 ArRobotConnector robotConnector(&parser, robot);
220 if (!robotConnector.connectRobot()) {
221 ArLog::log(ArLog::Terse,
"Could not connect to the robot");
222 if (parser.checkHelpAndWarnUnparsed()) {
227 if (!Aria::parseArgs()) {
233 std::cout <<
"Robot connected" << std::endl;
235 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
237 if (isInitialized ==
false) {
238 I.
resize((
unsigned int)half_size * 2, (
unsigned int)half_size * 2);
241 #if defined(VISP_HAVE_X11)
243 #elif defined(VISP_HAVE_GDI)
246 d->
init(I, -1, -1,
"Sonar range data");
247 isInitialized =
true;
252 ArGlobalFunctor sonarPrinterCB(&sonarPrinter);
253 robot->addRangeDevice(&sonar);
254 robot->addUserTask(
"Sonar printer", 50, &sonarPrinterCB);
261 for (
int i = 0; i < 1000; i++) {
265 std::cout <<
"Trans. vel= " << v_mes[0] <<
" m/s, Rot. vel=" <<
vpMath::deg(v_mes[1]) <<
" deg/s" << std::endl;
267 std::cout <<
"Left wheel vel= " << v_mes[0] <<
" m/s, Right wheel vel=" << v_mes[1] <<
" m/s" << std::endl;
268 std::cout <<
"Battery=" << robot->getBatteryVoltage() << std::endl;
270 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
278 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
280 #elif defined(_WIN32)
294 std::cerr << std::endl <<
"ERROR:" << std::endl;
295 std::cerr <<
" Cannot create " << opath << std::endl;
299 std::string filename = opath +
"/sonar.png";
312 ArLog::log(ArLog::Normal,
"simpleMotionCommands: Stopping.");
319 ArLog::log(ArLog::Normal,
320 "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. "
321 "Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
322 robot->getX(), robot->getY(), robot->getTh(), robot->getVel(), robot->getRotVel(),
323 robot->getBatteryVoltage());
326 std::cout <<
"Ending robot thread..." << std::endl;
327 robot->stopRunning();
329 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
337 robot->waitForRunExit();
342 ArLog::log(ArLog::Normal,
"simpleMotionCommands: Exiting.");
345 std::cout <<
"Catch an exception: " << e << std::endl;
Implementation of column vector and the associated operations.
static const vpColor blue
static const vpColor green
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...
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="") vp_override
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void getImage(const vpImage< unsigned char > &Is, vpImage< vpRGBa > &Id)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
static double rad(double deg)
static double deg(double rad)
Interface for Pioneer mobile robots based on Aria 3rd party library.
void useSonar(bool usage)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()