57 #include <visp/vpConfig.h>
58 #include <visp/vpDebug.h>
61 #ifdef VISP_HAVE_COIN_AND_GUI
62 #include <visp/vpImage.h>
63 #include <visp/vpCameraParameters.h>
64 #include <visp/vpTime.h>
65 #include <visp/vpSimulator.h>
69 #include <visp/vpMath.h>
70 #include <visp/vpHomogeneousMatrix.h>
71 #include <visp/vpFeatureEllipse.h>
72 #include <visp/vpCircle.h>
73 #include <visp/vpServo.h>
74 #include <visp/vpRobotCamera.h>
75 #include <visp/vpFeatureBuilder.h>
76 #include <visp/vpParseArgv.h>
77 #include <visp/vpIoTools.h>
80 #define GETOPTARGS "cdi:h"
93 void usage(
const char *name,
const char *badparam, std::string ipath)
96 Simulation Servo Circle\n\
99 %s [-i <input image path>] [-d] [-h]\n", name);
104 -i <input image path> %s\n\
105 Set image input path.\n\
106 From this path read \"ViSP-images/iv/4points.iv\"\n\
108 Setting the VISP_INPUT_IMAGE_PATH environment\n\
109 variable produces the same behaviour than using\n\
113 Disable the image display. This can be useful \n\
114 for automatic tests using crontab under Unix or \n\
115 using the task manager under Windows.\n\
118 Print the help.\n\n",
122 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
140 bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &display)
147 case 'i': ipath = optarg;
break;
148 case 'd': display =
false;
break;
149 case 'h': usage(argv[0], NULL, ipath);
return false;
break;
152 usage(argv[0], optarg, ipath);
return false;
break;
156 if ((c == 1) || (c == -1)) {
158 usage(argv[0], NULL, ipath);
159 std::cerr <<
"ERROR: " << std::endl;
160 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
168 void *mainLoop (
void *_simu)
189 unsigned int pos = 2 ;
197 float sampling_time = 0.040f;
209 vpTRACE(
"sets the initial camera location " ) ;
216 if (pos==1) cMod[2][3] = 0.32 ;
218 vpTRACE(
"sets the circle coordinates in the world frame " ) ;
222 vpTRACE(
"sets the desired position of the visual feature ") ;
227 vpTRACE(
"project : computes the circle coordinates in the camera frame and its 2D coordinates" ) ;
229 vpTRACE(
"sets the current position of the visual feature ") ;
235 vpTRACE(
"\t we want an eye-in-hand control law") ;
236 vpTRACE(
"\t robot is controlled in the camera frame") ;
240 vpTRACE(
"\t we want to see a circle on a circle..") ;
241 std::cout << std::endl ;
253 vpTRACE(
"Display task information " ) ;
258 std::cout <<
"\nEnter a character to continue... " <<std::endl ;
259 {
int a ; std::cin >> a ; }
262 unsigned int iter=0 ;
264 unsigned int itermax ;
265 if (pos==2) itermax = 75 ;
else itermax = 100 ;
266 char name[FILENAME_MAX] ;
267 while(iter++<itermax)
270 std::cout <<
"---------------------------------------------"
271 << iter <<std::endl ;
274 if (iter==1)
vpTRACE(
"\t\t get the robot position ") ;
276 if (iter==1)
vpTRACE(
"\t\t new circle position ") ;
282 if (iter==1)
vpTRACE(
"\t\t compute the control law ") ;
285 std::cout <<
"Task rank: " << task.
getTaskRank() <<std::endl ;
287 vpTRACE(
"\t\t send the camera velocity to the controller ") ;
294 sprintf(name,
"/tmp/image.%04d.external.png",it) ;
295 std::cout <<
"Save " << name << std::endl ;
297 sprintf(name,
"/tmp/image.%04d.internal.png",iter) ;
298 std::cout <<
"Save " << name << std::endl ;
322 main(
int argc,
const char ** argv)
324 std::string env_ipath;
325 std::string opt_ipath;
327 std::string filename;
328 std::string username;
329 bool opt_display =
true;
332 char *ptenv = getenv(
"VISP_INPUT_IMAGE_PATH");
337 if (! env_ipath.empty())
341 if (getOptions(argc, argv, opt_ipath, opt_display) ==
false) {
346 if (!opt_ipath.empty())
351 if (!opt_ipath.empty() && !env_ipath.empty()) {
352 if (ipath != env_ipath) {
353 std::cout << std::endl
354 <<
"WARNING: " << std::endl;
355 std::cout <<
" Since -i <visp image path=" << ipath <<
"> "
356 <<
" is different from VISP_INPUT_IMAGE_PATH=" << env_ipath << std::endl
357 <<
" we skip the environment variable." << std::endl;
362 if (opt_ipath.empty() && env_ipath.empty()){
363 usage(argv[0], NULL, ipath);
364 std::cerr << std::endl
365 <<
"ERROR:" << std::endl;
366 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
368 <<
" environment variable to specify the location of the " << std::endl
369 <<
" image path where test images are located." << std::endl << std::endl;
388 simu.
load(filename.c_str(),fMo) ;
402 {
vpTRACE(
"You should install Coin3D and SoQT or SoWin or SoXt") ;