52 #include <visp3/core/vpConfig.h>
53 #include <visp3/core/vpDebug.h>
55 #ifdef VISP_HAVE_COIN3D_AND_GUI
56 #include <visp3/ar/vpSimulator.h>
57 #include <visp3/core/vpCameraParameters.h>
58 #include <visp3/core/vpCircle.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/vpFeatureEllipse.h>
68 #include <visp3/vs/vpServo.h>
70 #define GETOPTARGS "cdi:h"
82 void usage(
const char *name,
const char *badparam, std::string ipath)
85 Simulation Servo Circle\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)
186 unsigned int pos = 2;
191 float sampling_time = 0.040f;
192 robot.setSamplingTime(sampling_time);
197 robot.setPosition(wMc);
225 std::cout << std::endl;
236 unsigned int iter = 0;
238 unsigned int itermax;
243 while (iter++ < itermax) {
247 std::cout <<
"get the robot position" << std::endl;
248 wMc = robot.getPosition();
250 std::cout <<
"new circle position" << std::endl;
258 std::cout <<
"compute the control law" << std::endl;
261 std::cout <<
"Task rank: " << task.
getTaskRank() << std::endl;
262 std::cout <<
"send the camera velocity to the controller" << std::endl;
269 char name[FILENAME_MAX];
270 sprintf(name,
"/tmp/image.%04d.external.png", it);
271 std::cout <<
"Save " << name << std::endl;
273 sprintf(name,
"/tmp/image.%04u.internal.png", iter);
274 std::cout <<
"Save " << name << std::endl;
291 int main(
int argc,
const char **argv)
294 std::string env_ipath;
295 std::string opt_ipath;
297 std::string filename;
298 bool opt_display =
true;
305 if (!env_ipath.empty())
309 if (getOptions(argc, argv, opt_ipath, opt_display) ==
false) {
314 if (!opt_ipath.empty())
319 if (!opt_ipath.empty() && !env_ipath.empty()) {
320 if (ipath != env_ipath) {
321 std::cout << std::endl <<
"WARNING: " << std::endl;
322 std::cout <<
" Since -i <visp image path=" << ipath <<
"> "
323 <<
" is different from VISP_INPUT_IMAGE_PATH=" << env_ipath << std::endl
324 <<
" we skip the environment variable." << std::endl;
329 if (opt_ipath.empty() && env_ipath.empty()) {
330 usage(argv[0], NULL, ipath);
331 std::cerr << std::endl <<
"ERROR:" << std::endl;
332 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
333 <<
" environment variable to specify the location of the " << std::endl
334 <<
" image path where test images are located." << std::endl
355 simu.
load(filename.c_str(), fMo);
364 std::cout <<
"Catch an exception: " << e << std::endl;
372 std::cout <<
"You do not have Coin3D and SoQT or SoWin or SoXt functionalities enabled..." << std::endl;
373 std::cout <<
"Tip:" << std::endl;
374 std::cout <<
"- Install Coin3D and SoQT or SoWin or SoXt, configure ViSP again using cmake and build again this example" << std::endl;
Generic class defining intrinsic camera parameters.
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
void setWorldCoordinates(const vpColVector &oP)
Implementation of column vector and the associated operations.
error that can be emited by ViSP classes.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines 2D ellipse visual feature.
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double rad(double deg)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Implementation of a pose vector and operations on poses.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void setMaxTranslationVelocity(double maxVt)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
unsigned int getTaskRank() const
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void setServo(const vpServoType &servo_type)
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Class that defines the simplest robot: a free flying camera.
Implementation of a simulator based on Coin3d (www.coin3d.org).
void load(const char *file_name)
load an iv file
void setInternalCameraParameters(vpCameraParameters &cam)
set internal camera parameters
virtual void mainLoop()
activate the mainloop
void initMainApplication()
perform some initialization in the main program thread
void initApplication(void *(*start_routine)(void *))
begin the main program
void setZoomFactor(float zoom)
set the size of the camera/frame
void setCameraPosition(vpHomogeneousMatrix &cMf)
set the camera position (from an homogeneous matrix)
void initExternalViewer(unsigned int nlig, unsigned int ncol)
initialize the external view
void write(const char *fileName)
virtual void initInternalViewer(unsigned int nlig, unsigned int ncol)
initialize the camera view
void closeMainApplication()
void addAbsoluteFrame(float zoom=1)
Add the representation of the absolute frame.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()