51 #include <visp3/core/vpConfig.h> 52 #include <visp3/core/vpDebug.h> 55 #ifdef VISP_HAVE_COIN3D_AND_GUI 57 #include <visp3/core/vpImage.h> 58 #include <visp3/core/vpCameraParameters.h> 59 #include <visp3/core/vpTime.h> 60 #include <visp3/ar/vpSimulator.h> 61 #include <visp3/core/vpMath.h> 62 #include <visp3/core/vpHomogeneousMatrix.h> 63 #include <visp3/visual_features/vpFeaturePoint.h> 64 #include <visp3/vs/vpServo.h> 65 #include <visp3/robot/vpSimulatorCamera.h> 66 #include <visp3/visual_features/vpFeatureBuilder.h> 67 #include <visp3/io/vpParseArgv.h> 68 #include <visp3/core/vpIoTools.h> 70 #define GETOPTARGS "di:h" 82 void usage(
const char *name,
const char *badparam, std::string ipath)
85 Simulation Servo 4points.\n\ 88 %s [-i <input image path>] [-d] [-h]\n", name);
92 -i <input image path> %s\n\ 93 Set image input path.\n\ 94 From this path read \"ViSP-images/iv/4points.iv\"\n\ 96 Setting the VISP_INPUT_IMAGE_PATH environment\n\ 97 variable produces the same behaviour than using\n\ 101 Disable the image display. This can be useful \n\ 102 for automatic tests using crontab under Unix or \n\ 103 using the task manager under Windows.\n\ 106 Print the help.\n\n", ipath.c_str());
109 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
127 bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &display)
134 case 'i': ipath = optarg;
break;
135 case 'd': display =
false;
break;
136 case 'h': usage(argv[0], NULL, ipath);
return false;
break;
139 usage(argv[0], optarg, ipath);
return false;
break;
143 if ((c == 1) || (c == -1)) {
145 usage(argv[0], NULL, ipath);
146 std::cerr <<
"ERROR: " << std::endl;
147 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
155 void *mainLoop (
void *_simu)
163 float sampling_time = 0.040f;
166 std::cout << std::endl ;
167 std::cout <<
"-------------------------------------------------------" << std::endl ;
168 std::cout <<
" Test program for vpServo " <<std::endl ;
169 std::cout <<
" Eye-in-hand task control, articular velocities are computed" << std::endl ;
170 std::cout <<
" Simulation " << std::endl ;
171 std::cout <<
" task : servo 4 points " << std::endl ;
172 std::cout <<
"-------------------------------------------------------" << std::endl ;
173 std::cout << std::endl ;
207 for (
int i = 0 ; i < 4 ; i++)
208 point[i].track(cMo) ;
212 for (
int i = 0 ; i < 4 ; i++)
240 for (
int i = 0 ; i < 4 ; i++)
246 std::cout <<
"Display task information" << std::endl;
251 unsigned int iter=0 ;
253 while(iter++ < 100) {
263 for (
int i = 0 ; i < 4 ; i++)
265 point[i].
track(cMo) ;
276 char name[FILENAME_MAX];
277 sprintf(name,
"/tmp/image.%04d.external.png",iter) ;
278 std::cout << name << std::endl ;
280 sprintf(name,
"/tmp/image.%04d.internal.png",iter) ;
286 std::cout <<
"\nDisplay task information" << std::endl;
296 int main(
int argc,
const char ** argv)
299 std::string env_ipath;
300 std::string opt_ipath;
302 std::string filename;
303 std::string username;
304 bool opt_display =
true;
310 if (! env_ipath.empty())
314 if (getOptions(argc, argv, opt_ipath, opt_display) ==
false) {
319 if (!opt_ipath.empty())
324 if (!opt_ipath.empty() && !env_ipath.empty()) {
325 if (ipath != env_ipath) {
326 std::cout << std::endl
327 <<
"WARNING: " << std::endl;
328 std::cout <<
" Since -i <visp image path=" << ipath <<
"> " 329 <<
" is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
330 <<
" we skip the environment variable." << std::endl;
335 if (opt_ipath.empty() && env_ipath.empty()){
336 usage(argv[0], NULL, ipath);
337 std::cerr << std::endl
338 <<
"ERROR:" << std::endl;
339 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " 341 <<
" environment variable to specify the location of the " << std::endl
342 <<
" image path where test images are located." << std::endl << std::endl;
359 simu.
load(filename.c_str()) ;
370 std::cout <<
"Catch an exception: " << e << std::endl;
378 {
vpTRACE(
"You should install Coin3D and SoQT or SoWin or SoXt") ;
void setPosition(const vpHomogeneousMatrix &wMc)
Implementation of a matrix and operations on matrices.
virtual void initInternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the camera view
VISP_EXPORT int wait(double t0, double t)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void write(const char *fileName)
void setExternalCameraParameters(vpCameraParameters &cam)
set external camera parameters
void setMaxTranslationVelocity(const double maxVt)
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
Implementation of a simulator based on Coin3d (www.coin3d.org).
void set_eJe(const vpMatrix &eJe_)
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 track(const vpHomogeneousMatrix &cMo)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
virtual void mainLoop()
activate the mainloop
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines what is a point.
virtual void setSamplingTime(const double &delta_t)
void initApplication(void *(*start_routine)(void *))
begin the main program
vpColVector computeControlLaw()
void getCameraPosition(vpHomogeneousMatrix &_cMf)
get the camera position (from an homogeneous matrix)
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
Generic class defining intrinsic camera parameters.
void load(const char *file_name)
load an iv file
vpHomogeneousMatrix getPosition() const
Implementation of a velocity twist matrix and operations on such kind of matrices.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
static double rad(double deg)
void buildFrom(const double x, const double y, const double Z)
VISP_EXPORT double measureTimeMs()
void initMainApplication()
perform some initialization in the main program thread
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Implementation of column vector and the associated operations.
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Implementation of a pose vector and operations on poses.
vpHomogeneousMatrix inverse() const
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void get_eJe(vpMatrix &eJe)
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