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>
66 #include <visp/vpMath.h>
67 #include <visp/vpHomogeneousMatrix.h>
68 #include <visp/vpFeatureEllipse.h>
69 #include <visp/vpCircle.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 "cdi:h"
88 void usage(
const char *name,
const char *badparam, std::string ipath)
91 Simulation Servo Circle\n\
94 %s [-i <input image path>] [-d] [-h]\n", name);
99 -i <input image path> %s\n\
100 Set image input path.\n\
101 From this path read \"ViSP-images/iv/4points.iv\"\n\
103 Setting the VISP_INPUT_IMAGE_PATH environment\n\
104 variable produces the same behaviour than using\n\
108 Disable the image display. This can be useful \n\
109 for automatic tests using crontab under Unix or \n\
110 using the task manager under Windows.\n\
113 Print the help.\n\n",
117 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
135 bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &display)
142 case 'i': ipath = optarg;
break;
143 case 'd': display =
false;
break;
144 case 'h': usage(argv[0], NULL, ipath);
return false;
break;
147 usage(argv[0], optarg, ipath);
return false;
break;
151 if ((c == 1) || (c == -1)) {
153 usage(argv[0], NULL, ipath);
154 std::cerr <<
"ERROR: " << std::endl;
155 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
163 void *mainLoop (
void *_simu)
184 unsigned int pos = 2 ;
190 float sampling_time = 0.040f;
198 if (pos==1) cMod[2][3] = 0.32 ;
222 std::cout << std::endl ;
233 unsigned int iter=0 ;
235 unsigned int itermax ;
236 if (pos==2) itermax = 75 ;
else itermax = 100 ;
237 while(iter++ < itermax)
241 if (iter==1) std::cout <<
"get the robot position" << std::endl;
243 if (iter==1) std::cout <<
"new circle position" << std::endl;
249 if (iter==1) std::cout <<
"compute the control law" << std::endl;
252 std::cout <<
"Task rank: " << task.
getTaskRank() <<std::endl ;
253 std::cout <<
"send the camera velocity to the controller" << std::endl;
261 char name[FILENAME_MAX] ;
262 sprintf(name,
"/tmp/image.%04d.external.png",it) ;
263 std::cout <<
"Save " << name << std::endl ;
265 sprintf(name,
"/tmp/image.%04d.internal.png",iter) ;
266 std::cout <<
"Save " << name << std::endl ;
287 main(
int argc,
const char ** argv)
290 std::string env_ipath;
291 std::string opt_ipath;
293 std::string filename;
294 std::string username;
295 bool opt_display =
true;
301 if (! env_ipath.empty())
305 if (getOptions(argc, argv, opt_ipath, opt_display) ==
false) {
310 if (!opt_ipath.empty())
315 if (!opt_ipath.empty() && !env_ipath.empty()) {
316 if (ipath != env_ipath) {
317 std::cout << std::endl
318 <<
"WARNING: " << std::endl;
319 std::cout <<
" Since -i <visp image path=" << ipath <<
"> "
320 <<
" is different from VISP_INPUT_IMAGE_PATH=" << env_ipath << std::endl
321 <<
" we skip the environment variable." << std::endl;
326 if (opt_ipath.empty() && env_ipath.empty()){
327 usage(argv[0], NULL, ipath);
328 std::cerr << std::endl
329 <<
"ERROR:" << std::endl;
330 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
332 <<
" environment variable to specify the location of the " << std::endl
333 <<
" image path where test images are located." << std::endl << std::endl;
352 simu.
load(filename.c_str(),fMo) ;
363 std::cout <<
"Catch an exception: " << e << std::endl;
372 {
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 setMaxTranslationVelocity(const double maxVt)
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)