55 #include <visp/vpConfig.h>
56 #include <visp/vpDebug.h>
59 #ifdef VISP_HAVE_COIN_AND_GUI
61 #include <visp/vpImage.h>
62 #include <visp/vpCameraParameters.h>
63 #include <visp/vpTime.h>
64 #include <visp/vpSimulator.h>
67 #include <visp/vpMath.h>
68 #include <visp/vpHomogeneousMatrix.h>
69 #include <visp/vpFeaturePoint.h>
70 #include <visp/vpServo.h>
71 #include <visp/vpRobotCamera.h>
72 #include <visp/vpFeatureBuilder.h>
73 #include <visp/vpParseArgv.h>
74 #include <visp/vpIoTools.h>
76 #define GETOPTARGS "di:h"
88 void usage(
const char *name,
const char *badparam, std::string ipath)
91 Simulation Servo 4points.\n\
94 %s [-i <input image path>] [-d] [-h]\n", name);
98 -i <input image path> %s\n\
99 Set image input path.\n\
100 From this path read \"ViSP-images/iv/4points.iv\"\n\
102 Setting the VISP_INPUT_IMAGE_PATH environment\n\
103 variable produces the same behaviour than using\n\
107 Disable the image display. This can be useful \n\
108 for automatic tests using crontab under Unix or \n\
109 using the task manager under Windows.\n\
112 Print the help.\n\n", ipath.c_str());
115 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
133 bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &display)
140 case 'i': ipath = optarg;
break;
141 case 'd': display =
false;
break;
142 case 'h': usage(argv[0], NULL, ipath);
return false;
break;
145 usage(argv[0], optarg, ipath);
return false;
break;
149 if ((c == 1) || (c == -1)) {
151 usage(argv[0], NULL, ipath);
152 std::cerr <<
"ERROR: " << std::endl;
153 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
161 void *mainLoop (
void *_simu)
172 float sampling_time = 0.040f;
175 std::cout << std::endl ;
176 std::cout <<
"-------------------------------------------------------" << std::endl ;
177 std::cout <<
" Test program for vpServo " <<std::endl ;
178 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl ;
179 std::cout <<
" Simulation " << std::endl ;
180 std::cout <<
" task : servo 4 points " << std::endl ;
181 std::cout <<
"-------------------------------------------------------" << std::endl ;
182 std::cout << std::endl ;
185 vpTRACE(
"sets the initial camera location " ) ;
204 vpTRACE(
"sets the point coordinates in the world frame " ) ;
211 vpTRACE(
"project : computes the point coordinates in the camera frame and its 2D coordinates" ) ;
212 for (i = 0 ; i < 4 ; i++)
213 point[i].track(cMo) ;
215 vpTRACE(
"sets the desired position of the point ") ;
217 for (i = 0 ; i < 4 ; i++)
221 vpTRACE(
"sets the desired position of the point ") ;
230 vpTRACE(
"\t we want an eye-in-hand control law") ;
231 vpTRACE(
"\t articular velocity are computed") ;
236 vpTRACE(
"Set the position of the camera in the end-effector frame ") ;
241 vpTRACE(
"Set the Jacobian (expressed in the end-effector frame)") ;
246 vpTRACE(
"\t we want to see a point on a point..") ;
247 for (i = 0 ; i < 4 ; i++)
254 vpTRACE(
"Display task information " ) ;
258 std::cout <<
"\nEnter a character to continue or CTRL-C to quit... "
260 {
char a ; std::cin >> a ; }
263 char name[FILENAME_MAX];
264 unsigned int iter=0 ;
266 while(iter++ < 100) {
275 for (i = 0 ; i < 4 ; i++)
277 point[i].
track(cMo) ;
292 sprintf(name,
"/tmp/image.%04d.external.png",iter) ;
293 std::cout << name << std::endl ;
295 sprintf(name,
"/tmp/image.%04d.internal.png",iter) ;
302 vpTRACE(
"Display task information " ) ;
305 std::cout <<
"\nEnter a character to continue..." <<std::endl ;
306 {
char a ; std::cin >> a ; }
319 main(
int argc,
const char ** argv)
321 std::string env_ipath;
322 std::string opt_ipath;
324 std::string filename;
325 std::string username;
326 bool opt_display =
true;
329 char *ptenv = getenv(
"VISP_INPUT_IMAGE_PATH");
334 if (! env_ipath.empty())
338 if (getOptions(argc, argv, opt_ipath, opt_display) ==
false) {
343 if (!opt_ipath.empty())
348 if (!opt_ipath.empty() && !env_ipath.empty()) {
349 if (ipath != env_ipath) {
350 std::cout << std::endl
351 <<
"WARNING: " << std::endl;
352 std::cout <<
" Since -i <visp image path=" << ipath <<
"> "
353 <<
" is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
354 <<
" we skip the environment variable." << std::endl;
359 if (opt_ipath.empty() && env_ipath.empty()){
360 usage(argv[0], NULL, ipath);
361 std::cerr << std::endl
362 <<
"ERROR:" << std::endl;
363 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
365 <<
" environment variable to specify the location of the " << std::endl
366 <<
" image path where test images are located." << std::endl << std::endl;
384 simu.
load(filename.c_str()) ;
397 {
vpTRACE(
"You should install Coin3D and SoQT or SoWin or SoXt") ;