 |
Visual Servoing Platform
version 3.3.0
|
40 #include <visp3/core/vpConfig.h>
42 #include <visp3/robot/vpRobotPioneer.h>
43 #include <visp3/blob/vpDot2.h>
44 #include <visp3/core/vpCameraParameters.h>
45 #include <visp3/core/vpHomogeneousMatrix.h>
46 #include <visp3/core/vpImage.h>
47 #include <visp3/core/vpVelocityTwistMatrix.h>
48 #include <visp3/gui/vpDisplayGDI.h>
49 #include <visp3/gui/vpPlot.h>
50 #include <visp3/robot/vpPioneerPan.h>
51 #include <visp3/robot/vpRobotBiclops.h>
52 #include <visp3/sensor/vp1394CMUGrabber.h>
53 #include <visp3/sensor/vp1394TwoGrabber.h>
54 #include <visp3/sensor/vpV4l2Grabber.h>
55 #include <visp3/visual_features/vpFeatureBuilder.h>
56 #include <visp3/visual_features/vpFeatureSegment.h>
57 #include <visp3/vs/vpServo.h>
58 #include <visp3/gui/vpDisplayX.h>
60 #define USE_REAL_ROBOT
62 #undef VISP_HAVE_V4L2 // To use a firewire camera
83 #if defined(VISP_HAVE_PIONEER) && defined(VISP_HAVE_BICLOPS)
84 int main(
int argc,
char **argv)
86 #if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394)
87 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
95 double coef = 1.2 / 13.0;
98 bool normalized =
true;
108 #ifdef USE_REAL_ROBOT
133 ArArgumentParser parser(&argc, argv);
134 parser.loadDefaultArguments();
138 ArRobotConnector robotConnector(&parser, &pioneer);
139 if (!robotConnector.connectRobot()) {
140 ArLog::log(ArLog::Terse,
"Could not connect to the pioneer robot.");
141 if (parser.checkHelpAndWarnUnparsed()) {
146 if (!Aria::parseArgs()) {
160 std::cout <<
"Pioneer robot connected" << std::endl;
171 #if defined(VISP_HAVE_V4L2)
181 #elif defined(VISP_HAVE_DC1394)
189 #elif defined(VISP_HAVE_CMU1394)
204 #if defined(VISP_HAVE_X11)
206 #elif defined(VISP_HAVE_GDI)
214 for (
int i = 0; i < 2; i++) {
234 std::cout <<
"cVe: \n" << cVe << std::endl;
243 std::cout <<
"eJe: \n" << eJe << std::endl;
252 for (
int i = 0; i < 2; i++) {
261 for (
int i = 0; i < 2; i++) {
264 surface[i] = 1. / sqrt(dot[i].m00 / (cam.
get_px() * cam.
get_py()));
267 Z[i] = coef * surface[i];
272 s_segment_d(normalized);
274 s_segment.setZ1(Z[0]);
275 s_segment.setZ2(Z[1]);
278 s_segment.setZ1(P[0].get_Z());
279 s_segment.setZ2(P[1].get_Z());
286 vpPlot graph(2, 500, 500, 700, 10,
"Curves...");
290 graph.initGraph(0, 3);
291 graph.initGraph(1, 3);
292 graph.setTitle(0,
"Velocities");
293 graph.setTitle(1,
"Error s-s*");
294 graph.setLegend(0, 0,
"vx");
295 graph.setLegend(0, 1,
"wz");
296 graph.setLegend(0, 2,
"w_pan");
297 graph.setLegend(1, 0,
"xm/l");
298 graph.setLegend(1, 1,
"1/l");
299 graph.setLegend(1, 2,
"alpha");
305 unsigned int iter = 0;
307 #ifdef USE_REAL_ROBOT
319 for (
int i = 0; i < 2; i++)
323 for (
int i = 0; i < 2; i++)
326 for (
int i = 0; i < 2; i++) {
329 surface[i] = 1. / sqrt(dot[i].m00 / (cam.
get_px() * cam.
get_py()));
332 Z[i] = coef * surface[i];
340 s_segment.setZ1(Z[0]);
341 s_segment.setZ2(Z[1]);
367 std::cout <<
"Warning: task is of rank " << task.
getTaskRank() << std::endl;
370 graph.plot(0, iter, v);
371 graph.plot(1, iter, task.
getError());
374 #ifdef USE_REAL_ROBOT
383 std::cout <<
"Send velocity to the pionner: " << v_pioneer[0] <<
" m/s " <<
vpMath::deg(v_pioneer[1])
384 <<
" deg/s" << std::endl;
385 std::cout <<
"Send velocity to the biclops head: " <<
vpMath::deg(v_biclops[0]) <<
" deg/s" << std::endl;
406 #ifdef USE_REAL_ROBOT
407 std::cout <<
"Ending robot thread..." << std::endl;
408 pioneer.stopRunning();
411 pioneer.waitForRunExit();
419 std::cout <<
"Catch an exception: " << e << std::endl;
428 std::cout <<
"ViSP is not able to control the Pioneer robot" << std::endl;
Initialize the velocity controller.
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
void open(vpImage< unsigned char > &I)
void setInput(unsigned input=vpV4l2Grabber::DEFAULT_INPUT)
static unsigned int selectAlpha()
static unsigned int selectXc()
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
void set_eJe(double q_pan)
void setDevice(const std::string &devname)
void useSonar(bool usage)
void acquire(vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
void open(vpImage< unsigned char > &I)
Generic functions for Pioneer mobile robots equiped with a pan head.
Interface for the biclops, pan, tilt head control.
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
void set_eJe(const vpMatrix &eJe_)
static double deg(double rad)
vpVelocityTwistMatrix get_cVe() const
Display for windows using GDI (available on any windows 32 platform).
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void setVideoMode(unsigned long format, unsigned long mode)
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
void set_cVe(const vpVelocityTwistMatrix &cVe_)
void setEllipsoidBadPointsPercentage(const double &percentage=0.0)
Implementation of column vector and the associated operations.
static unsigned int selectL()
Implementation of a matrix and operations on matrices.
void setServo(const vpServoType &servo_type)
Initialize the position controller.
void setFramerate(vpV4l2FramerateType framerate)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static void display(const vpImage< unsigned char > &I)
void setWorldCoordinates(double oX, double oY, double oZ)
void setComputeMoments(bool activate)
vpColVector getError() const
void setGrayLevelPrecision(const double &grayLevelPrecision)
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
unsigned int getTaskRank() const
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
Interface for Pioneer mobile robots based on Aria 3rd party library.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void setFramerate(unsigned long fps)
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
vpColVector computeControlLaw()
static void flush(const vpImage< unsigned char > &I)
void setEllipsoidShapePrecision(const double &ellipsoidShapePrecision)
Class that defines what is a point.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
Implementation of an homogeneous matrix and operations on such kind of matrices.
error that can be emited by ViSP classes.
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
void setGraphics(bool activate)