55 #include <visp/vpDebug.h>
56 #include <visp/vpConfig.h>
58 #if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
63 #include <visp/vpMath.h>
64 #include <visp/vpHomogeneousMatrix.h>
65 #include <visp/vpFeatureLine.h>
66 #include <visp/vpLine.h>
67 #include <visp/vpServo.h>
68 #include <visp/vpRobotCamera.h>
69 #include <visp/vpFeatureBuilder.h>
73 #include <visp/vpException.h>
74 #include <visp/vpMatrixException.h>
77 #include <visp/vpDebug.h>
80 #include <visp/vpServoDisplay.h>
82 #include <visp/vpImage.h>
83 #include <visp/vpDisplayX.h>
84 #include <visp/vpDisplayGTK.h>
85 #include <visp/vpDisplayGDI.h>
86 #include <visp/vpCameraParameters.h>
87 #include <visp/vpParseArgv.h>
91 #define GETOPTARGS "cdh"
101 void usage(
const char *name,
const char *badparam)
104 Simulation of 2D a visual servoing on a line:\n\
105 - eye-in-hand control law,\n\
106 - velocity computed in the camera frame,\n\
107 - display the camera view.\n\
110 %s [-c] [-d] [-h]\n", name);
116 Disable the mouse click. Useful to automaze the \n\
117 execution of this program without humain intervention.\n\
120 Turn off the display.\n\
126 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
141 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
148 case 'c': click_allowed =
false;
break;
149 case 'd': display =
false;
break;
150 case 'h': usage(argv[0], NULL);
return false;
break;
153 usage(argv[0], optarg);
158 if ((c == 1) || (c == -1)) {
160 usage(argv[0], NULL);
161 std::cerr <<
"ERROR: " << std::endl;
162 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
171 main(
int argc,
const char ** argv)
173 bool opt_display =
true;
174 bool opt_click_allowed =
true;
177 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
184 #if defined VISP_HAVE_X11
186 #elif defined VISP_HAVE_GTK
188 #elif defined VISP_HAVE_GDI
195 display.
init(I, 100, 100,
"Camera view...") ;
212 double px, py ; px = py = 600 ;
213 double u0, v0 ; u0 = v0 = 256 ;
220 vpTRACE(
"sets the initial camera location " ) ;
226 vpTRACE(
"sets the final camera location (for simulation purpose)" ) ;
233 vpTRACE(
"sets the line coordinates (2 planes) in the world frame " ) ;
243 vpTRACE(
"sets the desired position of the visual feature ") ;
244 for(
int i = 0; i < nbline; i++)
246 line[i].
track(cMod) ;
253 vpTRACE(
"project : computes the line coordinates in the camera frame and its 2D coordinates" ) ;
254 vpTRACE(
"sets the current position of the visual feature ") ;
255 for(
int i = 0; i < nbline; i++)
265 vpTRACE(
"\t we want an eye-in-hand control law") ;
266 vpTRACE(
"\t robot is controlled in the camera frame") ;
273 vpTRACE(
"\t we want to see a four lines on four lines.\n") ;
275 for(
int i = 0; i < nbline; i++)
286 vpTRACE(
"Display task information " ) ;
289 if (opt_display && opt_click_allowed) {
290 std::cout <<
"\n\nClick in the camera view window to start..." << std::endl;
294 unsigned int iter=0 ;
298 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
301 if (iter==1)
vpTRACE(
"\t\t get the robot position ") ;
303 if (iter==1)
vpTRACE(
"\t\t new line position ") ;
306 for(
int i = 0; i < nbline; i++)
318 if (iter==1)
vpTRACE(
"\t\t compute the control law ") ;
321 if (iter==1)
vpTRACE(
"\t\t send the camera velocity to the controller ") ;
324 vpTRACE(
"\t\t || s - s* || ") ;
328 if (opt_display && opt_click_allowed) {
329 std::cout <<
"\nClick in the camera view window to end..." << std::endl;
333 vpTRACE(
"Display task information " ) ;
342 vpERROR_TRACE(
"You do not have X11, GTK or GDI display functionalities...");
static void display(vpServo &s, const vpCameraParameters &cam, vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
void print(const unsigned int select=FEATURE_ALL) const
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setWorldCoordinates(const double &A1, const double &B1, const double &C1, const double &D1, const double &A2, const double &B2, const double &C2, const double &D2)
Display for windows using GDI (available on any windows 32 platform).
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
void setLambda(double _lambda)
set the gain lambda
void track(const vpHomogeneousMatrix &cMo)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static void flush(const vpImage< unsigned char > &I)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines a line in the object frame, the camera frame and the image plane. All the parameters must be set in meter.
void kill()
destruction (memory deallocation if required)
virtual void print() const
vpColVector computeControlLaw()
compute the desired control law
Class that defines the simplest robot: a free flying camera.
static void display(const vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
Class that defines a 2D line visual feature which is composed by two parameters that are and ...
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
void getPosition(vpColVector &q)
void setPosition(const vpRobot::vpControlFrameType, const vpColVector &)
Set a displacement (frame has to be specified) in position control.
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
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 that provides a data structure for the column vectors as well as a set of operations on these v...
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
virtual bool getClick(bool blocking=true)=0
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class required to compute the visual servoing control law.
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.