Example that shows how to control the Pioneer mobile robot by IBVS visual servoing with respect to a blob. The current visual features that are used are s = (x, log(Z/Z*)). The desired one are s* = (x*, 0), with:
The degrees of freedom that are controlled are (vx, wz), where wz is the rotational velocity and vx the translational velocity of the mobile platform at point M located at the middle between the two wheels.
The feature x allows to control wy, while log(Z/Z*) allows to control vz. The value of x is measured thanks to a blob tracker. The value of Z is estimated from the surface of the blob that is proportional to the depth Z.
#include <iostream>
#include <visp3/robot/vpRobotPioneer.h>
#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/sensor/vp1394CMUGrabber.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/sensor/vpOpenCVGrabber.h>
#include <visp3/sensor/vpV4l2Grabber.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureDepth.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394) || \
(VISP_HAVE_OPENCV_VERSION >= 0x020100)
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
#if defined(VISP_HAVE_PIONEER)
#define TEST_COULD_BE_ACHIEVED
#endif
#endif
#endif
#undef VISP_HAVE_OPENCV // To use a firewire camera
#undef VISP_HAVE_V4L2 // To use a firewire camera
#ifdef TEST_COULD_BE_ACHIEVED
int main(int argc, char **argv)
{
try {
double depth = 1.;
double lambda = 0.6;
double coef = 1. / 6.77;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobotConnector robotConnector(&parser, &robot);
if (!robotConnector.connectRobot()) {
ArLog::log(ArLog::Terse, "Could not connect to the robot.");
if (parser.checkHelpAndWarnUnparsed()) {
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs()) {
Aria::logOptions();
Aria::shutdown();
return false;
}
std::cout << "Robot connected" << std::endl;
#if defined(VISP_HAVE_OPENCV)
int device = 1;
std::cout << "Use device: " << device << std::endl;
cv::VideoCapture g(device);
g.set(CV_CAP_PROP_FRAME_WIDTH, 640);
g.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
if (!g.isOpened())
return -1;
cv::Mat frame;
g >> frame;
#elif defined(VISP_HAVE_V4L2)
#elif defined(VISP_HAVE_DC1394)
#elif defined(VISP_HAVE_CMU1394)
#endif
#if defined(VISP_HAVE_OPENCV)
g >> frame;
#else
#endif
#if defined(VISP_HAVE_X11)
#elif defined(VISP_HAVE_GDI)
#endif
double Z, Zd;
Z = coef * surface;
Zd = Z;
0);
while (1) {
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
g >> frame;
#else
#endif
Z = coef * surface;
v = -lambda * (L * cVe * eJe).pseudoInverse() * error;
std::cout <<
"Send velocity to the pionner: " << v[0] <<
" m/s " <<
vpMath::deg(v[1]) <<
" deg/s" << std::endl;
break;
}
std::cout << "Ending robot thread..." << std::endl;
robot.stopRunning();
robot.waitForRunExit();
return EXIT_SUCCESS;
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
#else
int main()
{
std::cout << "You don't have the right 3rd party libraries to run this example..." << std::endl;
return EXIT_SUCCESS;
}
#endif