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 ;
195 float sampling_time = 0.040f;
207 vpTRACE(
"sets the initial camera location " ) ;
214 if (pos==1) cMod[2][3] = 0.32 ;
216 vpTRACE(
"sets the circle coordinates in the world frame " ) ;
220 vpTRACE(
"sets the desired position of the visual feature ") ;
225 vpTRACE(
"project : computes the circle coordinates in the camera frame and its 2D coordinates" ) ;
227 vpTRACE(
"sets the current position of the visual feature ") ;
233 vpTRACE(
"\t we want an eye-in-hand control law") ;
234 vpTRACE(
"\t robot is controlled in the camera frame") ;
238 vpTRACE(
"\t we want to see a circle on a circle..") ;
239 std::cout << std::endl ;
251 vpTRACE(
"Display task information " ) ;
256 std::cout <<
"\nEnter a character to continue... " <<std::endl ;
257 {
int a ; std::cin >> a ; }
260 unsigned int iter=0 ;
262 unsigned int itermax ;
263 if (pos==2) itermax = 75 ;
else itermax = 100 ;
264 char name[FILENAME_MAX] ;
265 while(iter++<itermax)
268 std::cout <<
"---------------------------------------------"
269 << iter <<std::endl ;
272 if (iter==1)
vpTRACE(
"\t\t get the robot position ") ;
274 if (iter==1)
vpTRACE(
"\t\t new circle position ") ;
280 if (iter==1)
vpTRACE(
"\t\t compute the control law ") ;
283 std::cout <<
"Task rank: " << task.
getTaskRank() <<std::endl ;
285 vpTRACE(
"\t\t send the camera velocity to the controller ") ;
292 sprintf(name,
"/tmp/image.%04d.external.png",it) ;
293 std::cout <<
"Save " << name << std::endl ;
295 sprintf(name,
"/tmp/image.%04d.internal.png",iter) ;
296 std::cout <<
"Save " << name << std::endl ;
320 main(
int argc,
const char ** argv)
323 std::string env_ipath;
324 std::string opt_ipath;
326 std::string filename;
327 std::string username;
328 bool opt_display =
true;
331 char *ptenv = getenv(
"VISP_INPUT_IMAGE_PATH");
336 if (! env_ipath.empty())
340 if (getOptions(argc, argv, opt_ipath, opt_display) ==
false) {
345 if (!opt_ipath.empty())
350 if (!opt_ipath.empty() && !env_ipath.empty()) {
351 if (ipath != env_ipath) {
352 std::cout << std::endl
353 <<
"WARNING: " << std::endl;
354 std::cout <<
" Since -i <visp image path=" << ipath <<
"> "
355 <<
" is different from VISP_INPUT_IMAGE_PATH=" << env_ipath << std::endl
356 <<
" we skip the environment variable." << std::endl;
361 if (opt_ipath.empty() && env_ipath.empty()){
362 usage(argv[0], NULL, ipath);
363 std::cerr << std::endl
364 <<
"ERROR:" << std::endl;
365 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
367 <<
" environment variable to specify the location of the " << std::endl
368 <<
" image path where test images are located." << std::endl << std::endl;
387 simu.
load(filename.c_str(),fMo) ;
398 std::cout <<
"Catch an exception: " << e << std::endl;
407 {
vpTRACE(
"You should install Coin3D and SoQT or SoWin or SoXt") ;
virtual void initInternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the camera view
void write(const char *fileName)
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setPosition(const vpHomogeneousMatrix &cMw)
Implementation of a simulator based on Coin3d (www.coin3d.org).
void closeMainApplication()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
error that can be emited by ViSP classes.
void addAbsoluteFrame(float zoom=1)
Add the representation of the absolute frame.
void track(const vpHomogeneousMatrix &cMo)
static double measureTimeMs()
virtual void mainLoop()
activate the mainloop
static int wait(double t0, double t)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
virtual void setSamplingTime(const double &delta_t)
void initApplication(void *(*start_routine)(void *))
begin the main program
vpColVector computeControlLaw()
Class that defines the simplest robot: a free flying camera.
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
Generic class defining intrinsic camera parameters.
void load(const char *file_name)
load an iv file
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
static double rad(double deg)
void getPosition(vpHomogeneousMatrix &cMw) const
void initMainApplication()
perform some initialization in the main program thread
Class that provides a data structure for the column vectors as well as a set of operations on these v...
The pose is a complete representation of every rigid motion in the euclidian space.
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Class that defines 2D ellipse visual feature.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines what is a circle.
void initExternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the external view
void setServo(const vpServoType &servo_type)
void setZoomFactor(const float zoom)
set the size of the camera/frame
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
unsigned int getTaskRank() const
void setWorldCoordinates(const vpColVector &oP)