43 #include <visp/vpConfig.h>
45 #include <visp/vpRobotPioneer.h>
46 #include <visp/vpRobotBiclops.h>
47 #include <visp/vpCameraParameters.h>
48 #include <visp/vpDisplayGDI.h>
49 #include <visp/vpDisplayX.h>
50 #include <visp/vpDot2.h>
51 #include <visp/vpFeatureBuilder.h>
52 #include <visp/vpFeatureSegment.h>
53 #include <visp/vpHomogeneousMatrix.h>
54 #include <visp/vpImage.h>
55 #include <visp/vp1394TwoGrabber.h>
56 #include <visp/vp1394CMUGrabber.h>
57 #include <visp/vpV4l2Grabber.h>
58 #include <visp/vpPioneerPan.h>
59 #include <visp/vpPlot.h>
60 #include <visp/vpServo.h>
61 #include <visp/vpVelocityTwistMatrix.h>
63 #define USE_REAL_ROBOT
83 #if defined(VISP_HAVE_PIONEER) && defined(VISP_HAVE_BICLOPS)
84 int main(
int argc,
char **argv)
86 #if defined(VISP_HAVE_DC1394_2) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394)
87 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
91 double coef = 0.9/14.85;
102 #ifdef USE_REAL_ROBOT
126 ArArgumentParser parser(&argc, argv);
127 parser.loadDefaultArguments();
131 ArRobotConnector robotConnector(&parser, &pioneer);
132 if(!robotConnector.connectRobot())
134 ArLog::log(ArLog::Terse,
"Could not connect to the pioneer robot.");
135 if(parser.checkHelpAndWarnUnparsed())
141 if (!Aria::parseArgs())
150 std::cout <<
"Pioneer robot connected" << std::endl;
156 #if defined(VISP_HAVE_V4L2)
163 #elif defined(VISP_HAVE_DC1394_2)
168 #elif defined(VISP_HAVE_CMU1394)
180 #if defined(VISP_HAVE_X11)
182 #elif defined(VISP_HAVE_GDI)
190 for (
int i=0; i <2; i++)
213 std::cout <<
"cVe: \n" << cVe << std::endl;
222 std::cout <<
"eJe: \n" << eJe << std::endl;
230 for (
int i=0; i <2; i++)
239 for (
int i=0; i<2; i++)
242 surface[i] = 1./sqrt(dot[i].m00/(cam.
get_px()*cam.
get_py()));
245 Z[i] = coef * surface[i] ;
249 bool normalized =
true;
252 s_segment.setZ1(Z[0]);
253 s_segment.setZ2(Z[1]);
256 s_segment.setZ1( P[0].get_Z() );
257 s_segment.setZ2( P[1].get_Z() );
266 vpPlot graph(2, 500, 500, 700, 10,
"Curves...");
269 graph.initGraph(0,3);
270 graph.initGraph(1,3);
271 graph.setTitle(0,
"Velocities");
272 graph.setTitle(1,
"Error s-s*");
273 graph.setLegend(0, 0,
"vx");
274 graph.setLegend(0, 1,
"wz");
275 graph.setLegend(0, 2,
"w_pan");
276 graph.setLegend(1, 0,
"xm/l");
277 graph.setLegend(1, 1,
"1/l");
278 graph.setLegend(1, 2,
"alpha");
282 unsigned int iter = 0;
287 #ifdef USE_REAL_ROBOT
299 for (
int i=0; i<2; i++)
303 for (
int i=0; i<2; i++)
306 for (
int i=0; i<2; i++)
309 surface[i] = 1./sqrt(dot[i].m00/(cam.
get_px()*cam.
get_py()));
312 Z[i] = coef * surface[i] ;
319 s_segment.setZ1(Z[0]);
320 s_segment.setZ2(Z[1]);
344 std::cout <<
"Warning: task is of rank " << task.
getTaskRank() << std::endl;
347 graph.plot(0, iter, v);
348 graph.plot(1, iter, task.
getError());
351 #ifdef USE_REAL_ROBOT
380 #ifdef USE_REAL_ROBOT
381 std::cout <<
"Ending robot thread..." << std::endl;
382 pioneer.stopRunning();
385 pioneer.waitForRunExit();
398 std::cout <<
"ViSP is not able to control the Pioneer robot" << std::endl;
Definition of the vpMatrix class.
static unsigned int selectL()
void setVideoMode(unsigned long format, unsigned long mode)
void open(vpImage< unsigned char > &I)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void open(vpImage< unsigned char > &I)
void useSonar(bool usage)
Display for windows using GDI (available on any windows 32 platform).
void setEllipsoidBadPointsPercentage(const double &percentage=0.0)
vpVelocityTwistMatrix get_cVe() const
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Define the X11 console to display images.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
create a new ste of two visual features
static unsigned int selectAlpha()
void setDevice(const char *devname)
void setLambda(double _lambda)
set the gain lambda
Initialize the position controller.
Interface for Pioneer mobile robots based on Aria 3rd party library.
Generic functions for Pioneer mobile robots equiped with a pan head.
This tracker is meant to track a blob (connex pixels with same gray level) on a vpImage.
void set_cVe(vpVelocityTwistMatrix &_cVe)
static void flush(const vpImage< unsigned char > &I)
Class that defines what is a point.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
void setGrayLevelPrecision(const double &grayLevelPrecision)
void kill()
destruction (memory deallocation if required)
Initialize the velocity controller.
vpColVector getError() const
Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
void setFramerate(unsigned long fps)
vpColVector computeControlLaw()
compute the desired control law
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
void acquire(vpImage< unsigned char > &I)
static void display(const vpImage< unsigned char > &I)
void set_eJe(vpMatrix &_eJe)
Generic class defining intrinsic camera parameters.
void setComputeMoments(const bool activate)
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
Interface for the biclops, pan, tilt head control.
void setEllipsoidShapePrecision(const double &ellipsoidShapePrecision)
void setInput(unsigned input=vpV4l2Grabber::DEFAULT_INPUT)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
static double rad(double deg)
Class for the Video4Linux2 video device.
Class that provides a data structure for the column vectors as well as a set of operations on these v...
double getTaskRank() const
void initTracking(const vpImage< unsigned char > &I, unsigned int size=0)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Class for firewire ieee1394 video devices using libdc1394-2.x api.
virtual bool getClick(bool blocking=true)=0
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
Class required to compute the visual servoing control law.
void set_eJe(double q_pan)
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
void setGraphics(const bool activate)
static unsigned int selectXc()
void setFramerate(vpV4l2FramerateType framerate)
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...