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/core/vpConfig.h>
#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpCameraParameters.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/robot/vpRobotPioneer.h>
#include <visp3/sensor/vp1394CMUGrabber.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/sensor/vpV4l2Grabber.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureDepth.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#if defined(HAVE_OPENCV_VIDEOIO)
#include <opencv2/videoio.hpp>
#endif
#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394) || defined(HAVE_OPENCV_VIDEOIO)
#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
#undef VISP_HAVE_V4L2
#ifdef TEST_COULD_BE_ACHIEVED
int main(int argc, char **argv)
{
#ifdef ENABLE_VISP_NAMESPACE
#endif
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(HAVE_OPENCV_VIDEOIO)
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 EXIT_FAILURE;
cv::Mat frame;
g >> frame;
#elif defined(VISP_HAVE_V4L2)
g.setInput(0);
g.setDevice("/dev/video1");
g.open(I);
#elif defined(VISP_HAVE_DC1394)
#elif defined(VISP_HAVE_CMU1394)
g.setFramerate(4);
g.open(I);
#endif
#if defined(HAVE_OPENCV_VIDEOIO)
g >> frame;
#else
g.acquire(I);
#endif
#if defined(VISP_HAVE_X11)
vpDisplayX d(I, 10, 10, "Current frame");
#elif defined(VISP_HAVE_GDI)
#endif
std::cout << "cVe: \n" << cVe << std::endl;
std::cout << "eJe: \n" << eJe << std::endl;
double surface = 1. / sqrt(dot.
m00 / (cam.
get_px() * cam.
get_py()));
double Z, Zd;
Z = coef * surface;
Zd = Z;
std::cout << "Z " << Z << std::endl;
0);
0);
while (1) {
#if defined(HAVE_OPENCV_VIDEOIO)
g >> frame;
#else
g.acquire(I);
#endif
surface = 1. / sqrt(dot.m00 / (cam.
get_px() * cam.
get_py()));
Z = coef * surface;
std::cout <<
"Send velocity to the pioneer: " << 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
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
void setVideoMode(unsigned long format, unsigned long mode)
Class for firewire ieee1394 video devices using libdc1394-2.x api.
@ vpVIDEO_MODE_640x480_MONO8
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
Display for windows using GDI (available on any windows 32 platform).
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 flush(const vpImage< unsigned char > &I)
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
void track(const vpImage< unsigned char > &I, bool canMakeTheWindowGrow=true)
void setGraphics(bool activate)
void setGrayLevelPrecision(const double &grayLevelPrecision)
void setEllipsoidBadPointsPercentage(const double &percentage=0.0)
void setEllipsoidShapePrecision(const double &ellipsoidShapePrecision)
void setComputeMoments(bool activate)
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
error that can be emitted by ViSP classes.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpImagePoint &t)
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
vpFeatureDepth & buildFrom(const double &x, const double &y, const double &Z, const double &LogZoverZstar)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
vpFeaturePoint & buildFrom(const double &x, const double &y, const double &Z)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
Interface for Pioneer mobile robots based on Aria 3rd party library.
void get_eJe(vpMatrix &eJe) VP_OVERRIDE
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void set_eJe(const vpMatrix &eJe_)
void setServo(const vpServoType &servo_type)
vpColVector computeControlLaw()
vpVelocityTwistMatrix get_cVe() const
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
VISP_EXPORT void sleepMs(double t)