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") ;
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)
create a new ste of two visual features
void setLambda(double _lambda)
set the gain lambda
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 kill()
destruction (memory deallocation if required)
void initApplication(void *(*start_routine)(void *))
begin the main program
vpColVector computeControlLaw()
compute the desired control law
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)
Set the type of the interaction matrix (current, mean, desired, user).
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.
double getTaskRank() const
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 required to compute the visual servoing control law descbribed in and .
Class that defines what is a circle.
void initExternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the external view
void setZoomFactor(const float zoom)
set the size of the camera/frame
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
void setWorldCoordinates(const vpColVector &oP)