 |
Visual Servoing Platform
version 3.2.0
|
52 #include <visp3/core/vpConfig.h>
53 #include <visp3/core/vpDebug.h>
55 #ifdef VISP_HAVE_COIN3D_AND_GUI
57 #include <visp3/ar/vpSimulator.h>
58 #include <visp3/core/vpCameraParameters.h>
59 #include <visp3/core/vpHomogeneousMatrix.h>
60 #include <visp3/core/vpImage.h>
61 #include <visp3/core/vpIoTools.h>
62 #include <visp3/core/vpMath.h>
63 #include <visp3/core/vpTime.h>
64 #include <visp3/io/vpParseArgv.h>
65 #include <visp3/robot/vpSimulatorCamera.h>
66 #include <visp3/visual_features/vpFeatureBuilder.h>
67 #include <visp3/visual_features/vpFeaturePoint.h>
68 #include <visp3/vs/vpServo.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 \"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)
141 usage(argv[0], NULL, ipath);
146 usage(argv[0], optarg, ipath);
152 if ((c == 1) || (c == -1)) {
154 usage(argv[0], NULL, ipath);
155 std::cerr <<
"ERROR: " << std::endl;
156 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
163 static void *mainLoop(
void *_simu)
165 vpSimulator *simu = static_cast<vpSimulator *>(_simu);
171 float sampling_time = 0.040f;
174 std::cout << std::endl;
175 std::cout <<
"-------------------------------------------------------" << std::endl;
176 std::cout <<
" Test program for vpServo " << std::endl;
177 std::cout <<
" Eye-in-hand task control, articular velocities are computed" << std::endl;
178 std::cout <<
" Simulation " << std::endl;
179 std::cout <<
" task : servo 4 points " << std::endl;
180 std::cout <<
"-------------------------------------------------------" << std::endl;
181 std::cout << std::endl;
216 for (
int i = 0; i < 4; i++)
221 for (
int i = 0; i < 4; i++)
249 for (
int i = 0; i < 4; i++)
255 std::cout <<
"Display task information" << std::endl;
260 unsigned int iter = 0;
262 while (iter++ < 100) {
272 for (
int i = 0; i < 4; i++) {
283 char name[FILENAME_MAX];
284 sprintf(name,
"/tmp/image.%04u.external.png", iter);
285 std::cout << name << std::endl;
287 sprintf(name,
"/tmp/image.%04u.internal.png", iter);
293 std::cout <<
"\nDisplay task information" << std::endl;
303 int main(
int argc,
const char **argv)
306 std::string env_ipath;
307 std::string opt_ipath;
309 std::string filename;
310 bool opt_display =
true;
317 if (!env_ipath.empty())
321 if (getOptions(argc, argv, opt_ipath, opt_display) ==
false) {
326 if (!opt_ipath.empty())
331 if (!opt_ipath.empty() && !env_ipath.empty()) {
332 if (ipath != env_ipath) {
333 std::cout << std::endl <<
"WARNING: " << std::endl;
334 std::cout <<
" Since -i <visp image path=" << ipath <<
"> "
335 <<
" is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
336 <<
" we skip the environment variable." << std::endl;
341 if (opt_ipath.empty() && env_ipath.empty()) {
342 usage(argv[0], NULL, ipath);
343 std::cerr << std::endl <<
"ERROR:" << std::endl;
344 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
345 <<
" environment variable to specify the location of the " << std::endl
346 <<
" image path where test images are located." << std::endl
365 simu.
load(filename.c_str());
375 std::cout <<
"Catch an exception: " << e << std::endl;
383 std::cout <<
"You do not have Coin3D and SoQT or SoWin or SoXt functionalities enabled..." << std::endl;
384 std::cout <<
"Tip:" << std::endl;
385 std::cout <<
"- Install Coin3D and SoQT or SoWin or SoXt, configure ViSP again using cmake and build again this example" << std::endl;
void setWorldCoordinates(const double oX, const double oY, const double oZ)
virtual void mainLoop()
activate the mainloop
void setMaxTranslationVelocity(const double maxVt)
VISP_EXPORT int wait(double t0, double t)
void buildFrom(const double x, const double y, const double Z)
static double rad(double deg)
Generic class defining intrinsic camera parameters.
void setZoomFactor(const float zoom)
set the size of the camera/frame
void set_eJe(const vpMatrix &eJe_)
void setPosition(const vpHomogeneousMatrix &wMc)
virtual void initInternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the camera view
void initApplication(void *(*start_routine)(void *))
begin the main program
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void load(const char *file_name)
load an iv file
void set_cVe(const vpVelocityTwistMatrix &cVe_)
void setExternalCameraParameters(vpCameraParameters &cam)
set external camera parameters
Class that defines the simplest robot: a free flying camera.
Implementation of column vector and the associated operations.
void initMainApplication()
perform some initialization in the main program thread
Implementation of a matrix and operations on matrices.
void setServo(const vpServoType &servo_type)
VISP_EXPORT double measureTimeMs()
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
vpHomogeneousMatrix getPosition() const
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Implementation of a pose vector and operations on poses.
void initExternalViewer(const unsigned int nlig, const unsigned int ncol)
initialize the external view
void get_eJe(vpMatrix &eJe)
void getCameraPosition(vpHomogeneousMatrix &_cMf)
get the camera position (from an homogeneous matrix)
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpColVector computeControlLaw()
void write(const char *fileName)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
virtual void setSamplingTime(const double &delta_t)
vpHomogeneousMatrix inverse() const
Class that defines what is a point.
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
Implementation of a simulator based on Coin3d (www.coin3d.org).
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
error that can be emited by ViSP classes.
void track(const vpHomogeneousMatrix &cMo)
void closeMainApplication()