 |
Visual Servoing Platform
version 3.2.0
|
61 #include <visp3/core/vpConfig.h>
62 #include <visp3/core/vpDebug.h>
63 #include <visp3/core/vpTime.h>
64 #if (defined(VISP_HAVE_BICLOPS) && (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_DIRECTSHOW)))
66 #ifdef VISP_HAVE_PTHREAD
70 #include <visp3/core/vpDisplay.h>
71 #include <visp3/core/vpImage.h>
72 #include <visp3/gui/vpDisplayGDI.h>
73 #include <visp3/gui/vpDisplayGTK.h>
74 #include <visp3/gui/vpDisplayX.h>
75 #include <visp3/sensor/vp1394TwoGrabber.h>
76 #include <visp3/sensor/vpDirectShowGrabber.h>
78 #include <visp3/blob/vpDot.h>
79 #include <visp3/core/vpHomogeneousMatrix.h>
80 #include <visp3/core/vpIoTools.h>
81 #include <visp3/core/vpMath.h>
82 #include <visp3/core/vpPoint.h>
83 #include <visp3/io/vpParseArgv.h>
84 #include <visp3/robot/vpRobotBiclops.h>
85 #include <visp3/visual_features/vpFeatureBuilder.h>
86 #include <visp3/visual_features/vpFeaturePoint.h>
87 #include <visp3/vs/vpServo.h>
88 #include <visp3/vs/vpServoDisplay.h>
91 #include <visp3/core/vpException.h>
93 #ifdef VISP_HAVE_PTHREAD
94 pthread_mutex_t mutexEndLoop = PTHREAD_MUTEX_INITIALIZER;
99 #ifdef VISP_HAVE_PTHREAD
100 pthread_mutex_unlock(&mutexEndLoop);
107 #define GETOPTARGS "c:d:h"
120 void usage(
const char *name,
const char *badparam, std::string &conf, std::string &debugdir, std::string &user)
123 Example of eye-in-hand control law. We control here a real robot, the biclops\n\
124 robot (pan-tilt head provided by Traclabs). The velocity is\n\
125 computed in articular. The visual feature is the center of gravity of a\n\
129 %s [-c <Biclops configuration file>] [-d <debug file directory>] [-h]\n", name);
133 -c <Biclops configuration file> %s\n\
134 Sets the biclops robot configuration file.\n\n\
135 -d <debug file directory> %s\n\
136 Sets the debug file directory.\n\
137 From this directory, creates the\"%s\"\n\
138 subdirectory depending on the username, where\n\
139 it writes biclops.txt file.\n", conf.c_str(), debugdir.c_str(), user.c_str());
142 fprintf(stderr,
"ERROR: \n");
143 fprintf(stderr,
"\nBad parameter [%s]\n", badparam);
159 bool getOptions(
int argc,
const char **argv, std::string &conf, std::string &debugdir, std::string &user)
173 usage(argv[0], NULL, conf, debugdir, user);
178 usage(argv[0], optarg_, conf, debugdir, user);
184 if ((c == 1) || (c == -1)) {
186 usage(argv[0], NULL, conf, debugdir, user);
187 std::cerr <<
"ERROR: " << std::endl;
188 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
195 int main(
int argc,
const char **argv)
197 std::cout << std::endl;
198 std::cout <<
"-------------------------------------------------------" << std::endl;
199 std::cout <<
" Test program for vpServo " << std::endl;
200 std::cout <<
" Eye-in-hand task control, velocity computed in the camera frame" << std::endl;
201 std::cout <<
" Simulation " << std::endl;
202 std::cout <<
" task : servo a point " << std::endl;
203 std::cout <<
"-------------------------------------------------------" << std::endl;
204 std::cout << std::endl;
208 #ifdef VISP_HAVE_PTHREAD
209 pthread_mutex_lock(&mutexEndLoop);
211 signal(SIGINT, &signalCtrC);
214 std::string opt_conf =
"/usr/share/BiclopsDefault.cfg";
216 std::string username;
217 std::string debugdir;
218 std::string opt_debugdir;
221 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
222 opt_debugdir =
"/tmp";
223 #elif defined(_WIN32)
224 opt_debugdir =
"C:/temp";
231 if (getOptions(argc, argv, opt_conf, opt_debugdir, username) ==
false) {
236 if (!opt_debugdir.empty())
237 debugdir = opt_debugdir;
240 std::string dirname = debugdir +
"/" + username;
248 usage(argv[0], NULL, opt_conf, debugdir, username);
249 std::cerr << std::endl <<
"ERROR:" << std::endl;
250 std::cerr <<
" Cannot create " << dirname << std::endl;
251 std::cerr <<
" Check your -d " << debugdir <<
" option " << std::endl;
257 char filename[FILENAME_MAX];
258 sprintf(filename,
"%s/biclops.txt", debugdir.c_str());
259 FILE *fd = fopen(filename,
"w");
273 #if defined VISP_HAVE_DC1394
275 #elif defined VISP_HAVE_DIRECTSHOW
290 #if defined VISP_HAVE_X11
291 vpDisplayX display(I, 100, 100,
"Display X...");
292 #elif defined VISP_HAVE_GTK
294 #elif defined(_WIN32)
311 std::cout <<
"Click on a dot to initialize the tracking..." << std::endl;
338 vpTRACE(
"Set the position of the camera in the end-effector frame ");
344 std::cout << cVe << std::endl;
347 std::cout <<
"Click in the image to start the servoing..." << std::endl;
366 unsigned int iter = 0;
368 #ifdef VISP_HAVE_PTHREAD
369 while (0 != pthread_mutex_trylock(&mutexEndLoop))
374 std::cout <<
"---------------------------------------------" << iter << std::endl;
398 std::cout <<
"v: " << v.
t();
401 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
405 s_minus_sStar = task.
s - task.
sStar;
406 fprintf(fd,
"%f %f %f %f %f\n", v[0], v[1], s_minus_sStar[0], s_minus_sStar[1], (task.
getError()).sumSquare());
410 std::cout <<
"Display task information " << std::endl;
419 std::cout <<
"Catch an exception: " << e.
getMessage() << std::endl;
427 std::cout <<
"You do not have an biclops PT robot connected to your computer..." << 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 acquire(vpImage< unsigned char > &I)
VISP_EXPORT int wait(double t0, double t)
void get_cVe(vpVelocityTwistMatrix &cVe) const
void buildFrom(const double x, const double y, const double Z)
Generic class defining intrinsic camera parameters.
Interface for the biclops, pan, tilt head control.
void set_eJe(const vpMatrix &eJe_)
void setPosition(const vpHomogeneousMatrix &wMc)
void track(const vpImage< unsigned char > &I)
Display for windows using GDI (available on any windows 32 platform).
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
void set_Z(const double Z)
Implementation of column vector and the associated operations.
Implementation of a matrix and operations on matrices.
void setServo(const vpServoType &servo_type)
Initialize the position controller.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static void display(const vpImage< unsigned char > &I)
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
void setGraphics(const bool activate)
vpColVector getError() const
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void get_eJe(vpMatrix &eJe)
const char * getMessage(void) const
class for windows direct show devices
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
static void flush(const vpImage< unsigned char > &I)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void initTracking(const vpImage< unsigned char > &I)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
error that can be emited by ViSP classes.
This tracker is meant to track a dot (connected pixels with same gray level) on a vpImage.