Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimuLine2DCamVelocityDisplay.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Simulation of a 2D visual servoing on a line.
33 *
34*****************************************************************************/
35
44#include <visp3/core/vpConfig.h>
45#include <visp3/core/vpDebug.h>
46
47#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
48 (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
49
50#include <stdio.h>
51#include <stdlib.h>
52
53#include <visp3/core/vpCameraParameters.h>
54#include <visp3/core/vpHomogeneousMatrix.h>
55#include <visp3/core/vpImage.h>
56#include <visp3/core/vpLine.h>
57#include <visp3/core/vpMath.h>
58#include <visp3/gui/vpDisplayGDI.h>
59#include <visp3/gui/vpDisplayGTK.h>
60#include <visp3/gui/vpDisplayOpenCV.h>
61#include <visp3/gui/vpDisplayX.h>
62#include <visp3/io/vpParseArgv.h>
63#include <visp3/robot/vpSimulatorCamera.h>
64#include <visp3/visual_features/vpFeatureBuilder.h>
65#include <visp3/visual_features/vpFeatureLine.h>
66#include <visp3/vs/vpServo.h>
67#include <visp3/vs/vpServoDisplay.h>
68
69// List of allowed command line options
70#define GETOPTARGS "cdh"
71
72void usage(const char *name, const char *badparam);
73bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
74
83void usage(const char *name, const char *badparam)
84{
85 fprintf(stdout, "\n\
86Simulation of 2D a visual servoing on a line:\n\
87- eye-in-hand control law,\n\
88- velocity computed in the camera frame,\n\
89- display the camera view.\n\
90 \n\
91SYNOPSIS\n\
92 %s [-c] [-d] [-h]\n",
93 name);
94
95 fprintf(stdout, "\n\
96OPTIONS: Default\n\
97 \n\
98 -c\n\
99 Disable the mouse click. Useful to automate the \n\
100 execution of this program without human intervention.\n\
101 \n\
102 -d \n\
103 Turn off the display.\n\
104 \n\
105 -h\n\
106 Print the help.\n");
107
108 if (badparam)
109 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
110}
111
124bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
125{
126 const char *optarg_;
127 int c;
128 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
129
130 switch (c) {
131 case 'c':
132 click_allowed = false;
133 break;
134 case 'd':
135 display = false;
136 break;
137 case 'h':
138 usage(argv[0], NULL);
139 return false;
140
141 default:
142 usage(argv[0], optarg_);
143 return false;
144 }
145 }
146
147 if ((c == 1) || (c == -1)) {
148 // standalone param or error
149 usage(argv[0], NULL);
150 std::cerr << "ERROR: " << std::endl;
151 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
152 return false;
153 }
154
155 return true;
156}
157
158int main(int argc, const char **argv)
159{
160 try {
161 bool opt_display = true;
162 bool opt_click_allowed = true;
163
164 // Read the command line options
165 if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
166 return EXIT_FAILURE;
167 }
168
169 vpImage<unsigned char> I(512, 512, 0);
170
171// We open a window using either X11, GTK or GDI.
172#if defined(VISP_HAVE_X11)
173 vpDisplayX display;
174#elif defined(VISP_HAVE_GTK)
175 vpDisplayGTK display;
176#elif defined(VISP_HAVE_GDI)
177 vpDisplayGDI display;
178#elif defined(HAVE_OPENCV_HIGHGUI)
179 vpDisplayOpenCV display;
180#endif
181
182 if (opt_display) {
183 try {
184 // Display size is automatically defined by the image (I) size
185 display.init(I, 100, 100, "Camera view...");
186 // Display the image
187 // The image class has a member that specify a pointer toward
188 // the display that has been initialized in the display declaration
189 // therefore is is no longer necessary to make a reference to the
190 // display variable.
193 } catch (...) {
194 vpERROR_TRACE("Error while displaying the image");
195 return EXIT_FAILURE;
196 }
197 }
198
199 double px = 500, py = 500;
200 double u0 = 150, v0 = 160;
201
202 vpCameraParameters cam(px, py, u0, v0);
203
204 vpServo task;
205 vpSimulatorCamera robot;
206
207 // sets the initial camera location
208 vpHomogeneousMatrix cMo(-0.2, 0.1, 1, vpMath::rad(5), vpMath::rad(5), vpMath::rad(90));
209
210 // Compute the position of the object in the world frame
211 vpHomogeneousMatrix wMc, wMo;
212 robot.getPosition(wMc);
213 wMo = wMc * cMo;
214
215 // sets the final camera location (for simulation purpose)
216 vpHomogeneousMatrix cMod(0, 0, 1, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
217
218 // sets the line coordinates (2 planes) in the world frame
219 vpColVector plane1(4);
220 vpColVector plane2(4);
221 plane1[0] = 0; // z = 0
222 plane1[1] = 0;
223 plane1[2] = 1;
224 plane1[3] = 0;
225 plane2[0] = 0; // y =0
226 plane2[1] = 1;
227 plane2[2] = 0;
228 plane2[3] = 0;
229
230 vpLine line;
231 line.setWorldCoordinates(plane1, plane2);
232
233 // sets the desired position of the visual feature
234 line.track(cMod);
235 line.print();
236
237 vpFeatureLine ld;
238 vpFeatureBuilder::create(ld, line);
239
240 // computes the line coordinates in the camera frame and its 2D
241 // coordinates sets the current position of the visual feature
242 line.track(cMo);
243 line.print();
244
247 l.print();
248
249 // define the task
250 // - we want an eye-in-hand control law
251 // - robot is controlled in the camera frame
253
254 // we want to see a line on a line
255
256 task.addFeature(l, ld);
258 vpServoDisplay::display(task, cam, I);
260
261 // set the gain
262 task.setLambda(1);
263 // Display task information " ) ;
264 task.print();
265
266 if (opt_display && opt_click_allowed) {
267 std::cout << "\n\nClick in the camera view window to start..." << std::endl;
269 }
270
271 unsigned int iter = 0;
272 // loop
273 while (iter++ < 200) {
274 std::cout << "---------------------------------------------" << iter << std::endl;
275 vpColVector v;
276
277 // get the robot position
278 robot.getPosition(wMc);
279 // Compute the position of the object frame in the camera frame
280 cMo = wMc.inverse() * wMo;
281
282 // new line position
283 line.track(cMo);
284 // retrieve x,y and Z of the vpLine structure
286
287 if (opt_display) {
289 vpServoDisplay::display(task, cam, I);
291 }
292
293 // compute the control law
294 v = task.computeControlLaw();
295
296 // send the camera velocity to the controller
298
299 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
300 }
301
302 if (opt_display && opt_click_allowed) {
303 vpDisplay::displayText(I, 20, 20, "Click to quit...", vpColor::white);
306 }
307
308 // Display task information
309 task.print();
310 return EXIT_SUCCESS;
311 } catch (const vpException &e) {
312 std::cout << "Catch a ViSP exception: " << e << std::endl;
313 return EXIT_FAILURE;
314 }
315}
316
317#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
318int main()
319{
320 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
321 return EXIT_SUCCESS;
322}
323#else
324int main()
325{
326 std::cout << "You do not have X11, or GTK, or GDI (Graphical Device Interface) functionalities to display images..."
327 << std::endl;
328 std::cout << "Tip if you are on a unix-like system:" << std::endl;
329 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
330 std::cout << "Tip if you are on a windows-like system:" << std::endl;
331 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
332 return EXIT_SUCCESS;
333}
334#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor white
Definition vpColor.h:206
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
Definition vpException.h:59
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D line visual feature which is composed by two parameters that are and ,...
void print(unsigned int select=FEATURE_ALL) const
virtual void print() const
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Definition of the vpImage class member functions.
Definition vpImage.h:135
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition vpLine.h:100
void setWorldCoordinates(const double &oA1, const double &oB1, const double &oC1, const double &oD1, const double &oA2, const double &oB2, const double &oC2, const double &oD2)
Definition vpLine.cpp:82
static double rad(double deg)
Definition vpMath.h:116
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ CAMERA_FRAME
Definition vpRobot.h:80
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
@ EYEINHAND_CAMERA
Definition vpServo.h:151
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition vpServo.cpp:299
void setLambda(double c)
Definition vpServo.h:403
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
vpColVector getError() const
Definition vpServo.h:276
vpColVector computeControlLaw()
Definition vpServo.cpp:930
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
Class that defines the simplest robot: a free flying camera.
#define vpERROR_TRACE
Definition vpDebug.h:388