#include <stdlib.h>
#include <stdio.h>
#include <visp3/core/vpCircle.h>
#include <visp3/visual_features/vpFeatureEllipse.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpSimulatorCamera.h>
#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
void usage(const char *name, const char *badparam)
{
fprintf(stdout, "\n\
Simulation of a 2D visual servoing on a circle:\n\
- eye-in-hand control law,\n\
- velocity computed in the camera frame,\n\
- without display.\n\
\n\
SYNOPSIS\n\
%s [-h]\n", name);
fprintf(stdout, "\n\
OPTIONS: Default\n\
\n\
-h\n\
Print the help.\n");
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
int c;
switch (c) {
case 'h': usage(argv[0], NULL); return false; break;
default:
usage(argv[0], optarg_);
return false; break;
}
}
if ((c == 1) || (c == -1)) {
usage(argv[0], NULL);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
return false;
}
return true;
}
int
main(int argc, const char ** argv)
{
try {
if (getOptions(argc, argv) == false) {
exit (-1);
}
std::cout << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << " Test program for vpServo " <<std::endl ;
std::cout << " Simulation " << std::endl ;
std::cout << " task : servo a circle " << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << std::endl ;
cMo[0][3] = 0.1 ;
cMo[1][3] = 0.2 ;
cMo[2][3] = 2 ;
wMo = wMc * cMo;
cMod[0][3] = 0 ;
cMod[1][3] = 0 ;
cMod[2][3] = 1 ;
std::cout << std::endl ;
unsigned int iter=0 ;
while(iter++ < 500)
{
std::cout << "---------------------------------------------" << iter <<std::endl ;
std::cout <<
"task rank: " << task.
getTaskRank() <<std::endl ;
std::cout <<
"|| s - s* || = " << ( task.
getError() ).sumSquare() << std::endl ;
}
return 0;
}
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}