Example that shows how to control the Pioneer mobile robot by IBVS visual servoing with respect to a segment. The segment consists in two horizontal dots. The current visual features that are used are . The desired one are , with:
The depth of the points is estimated from the surface of the blob.
#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/vpVelocityTwistMatrix.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpPlot.h>
#include <visp3/robot/vpPioneerPan.h>
#include <visp3/robot/vpRobotBiclops.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/vpFeatureSegment.h>
#include <visp3/vs/vpServo.h>
#define USE_REAL_ROBOT
#define USE_PLOTTER
#undef VISP_HAVE_V4L2
#if defined(VISP_HAVE_PIONEER) && defined(VISP_HAVE_BICLOPS)
int main(int argc, char **argv)
{
#ifdef ENABLE_VISP_NAMESPACE
#endif
#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394)
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
try {
double lambda = 0.1;
double coef = 1.2 / 13.0;
double L = 0.21;
double Z_d = 0.8;
bool normalized = true;
double Y_d = -.11;
qm = 0;
double qm_pan = 0;
#ifdef USE_REAL_ROBOT
q = 0;
qm_pan = qm[0];
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobotConnector robotConnector(&parser, &pioneer);
if (!robotConnector.connectRobot()) {
ArLog::log(ArLog::Terse, "Could not connect to the pioneer robot.");
if (parser.checkHelpAndWarnUnparsed()) {
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs()) {
Aria::logOptions();
Aria::shutdown();
return false;
}
sleep(3);
std::cout << "Pioneer robot connected" << std::endl;
#endif
#if defined(VISP_HAVE_V4L2)
#elif defined(VISP_HAVE_DC1394)
#elif defined(VISP_HAVE_CMU1394)
g.setVideoMode(0, 5);
#endif
#if defined(VISP_HAVE_X11)
vpDisplayX d(I, 10, 10, "Current frame");
#elif defined(VISP_HAVE_GDI)
#endif
for (int i = 0; i < 2; i++) {
}
std::cout << "cVe: \n" << cVe << std::endl;
std::cout << "eJe: \n" << eJe << std::endl;
0);
for (int i = 0; i < 2; i++) {
}
double surface[2];
double Z[2];
for (int i = 0; i < 2; i++) {
surface[i] = 1. / sqrt(dot[i].m00 / (cam.
get_px() * cam.
get_py()));
Z[i] = coef * surface[i];
}
s_segment_d(normalized);
s_segment.setZ1(Z[0]);
s_segment.setZ2(Z[1]);
s_segment.setZ1(P[0].get_Z());
s_segment.setZ2(P[1].get_Z());
#ifdef USE_PLOTTER
vpPlot graph(2, 500, 500, 700, 10,
"Curves...");
#endif
try {
unsigned int iter = 0;
while (1) {
#ifdef USE_REAL_ROBOT
#endif
qm_pan = qm[0];
for (int i = 0; i < 2; i++)
for (int i = 0; i < 2; i++)
dot[i].track(I);
for (int i = 0; i < 2; i++) {
surface[i] = 1. / sqrt(dot[i].m00 / (cam.
get_px() * cam.
get_py()));
Z[i] = coef * surface[i];
}
s_segment.setZ1(Z[0]);
s_segment.setZ2(Z[1]);
std::cout <<
"Warning: task is of rank " << task.
getTaskRank() << std::endl;
#ifdef USE_PLOTTER
#endif
#ifdef USE_REAL_ROBOT
v_pioneer[0] = v[0];
v_pioneer[1] = v[1];
v_biclops[0] = v[2];
v_biclops[1] = 0;
std::cout <<
"Send velocity to the pionner: " << v_pioneer[0] <<
" m/s " <<
vpMath::deg(v_pioneer[1])
<< " deg/s" << std::endl;
std::cout <<
"Send velocity to the Biclops head: " <<
vpMath::deg(v_biclops[0]) <<
" deg/s" << std::endl;
#endif
break;
iter++;
}
}
catch (...) {
}
#ifdef USE_REAL_ROBOT
std::cout << "Ending robot thread..." << std::endl;
pioneer.stopRunning();
pioneer.waitForRunExit();
#endif
return EXIT_SUCCESS;
}
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
#endif
#endif
}
#else
int main()
{
std::cout << "ViSP is not able to control the Pioneer robot" << std::endl;
return EXIT_SUCCESS;
}
#endif
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
Class for firewire ieee1394 video devices using libdc1394-2.x api.
@ vpVIDEO_MODE_640x480_MONO8
void setDenavitHartenbergModel(vpBiclops::DenavitHartenbergModel dh_model=vpBiclops::DH1)
@ DH1
First Denavit-Hartenberg representation.
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 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 2D segment visual features. This class allow to consider two sets of visual feat...
static unsigned int selectAlpha()
static unsigned int selectXc()
static unsigned int selectL()
Implementation of an homogeneous matrix and operations on such kind of matrices.
static double deg(double rad)
Implementation of a matrix and operations on matrices.
Generic functions for Pioneer mobile robots equipped with a pan head.
void set_eJe(double q_pan)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
void initGraph(unsigned int graphNum, unsigned int curveNbr)
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
void setTitle(unsigned int graphNum, const std::string &title)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const VP_OVERRIDE
void setWorldCoordinates(double oX, double oY, double oZ)
Interface for the Biclops, pan, tilt head control.
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState) VP_OVERRIDE
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) VP_OVERRIDE
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot) VP_OVERRIDE
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) VP_OVERRIDE
Interface for Pioneer mobile robots based on Aria 3rd party library.
void useSonar(bool usage)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
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_)
unsigned int getTaskRank() const
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 getError() const
vpColVector computeControlLaw()
vpVelocityTwistMatrix get_cVe() const
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
void setFramerate(vpV4l2FramerateType framerate)
void setInput(unsigned input=vpV4l2Grabber::DEFAULT_INPUT)
void open(vpImage< unsigned char > &I)
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
void setDevice(const std::string &devname)
void acquire(vpImage< unsigned char > &I)