Visual Servoing Platform version 3.5.0
tutorial-flir-ptu-ibvs.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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 http://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 * tests the control law
33 * eye-in-hand control
34 * velocity computed in the camera frame
35 *
36 * Authors:
37 * Fabien Spindler
38 *
39 *****************************************************************************/
61#include <iostream>
62
63#include <visp3/core/vpCameraParameters.h>
64#include <visp3/core/vpXmlParserCamera.h>
65#include <visp3/gui/vpDisplayGDI.h>
66#include <visp3/gui/vpDisplayX.h>
67#include <visp3/io/vpImageIo.h>
68#include <visp3/sensor/vpFlyCaptureGrabber.h>
69#include <visp3/robot/vpRobotFlirPtu.h>
70#include <visp3/detection/vpDetectorAprilTag.h>
71#include <visp3/visual_features/vpFeatureBuilder.h>
72#include <visp3/visual_features/vpFeaturePoint.h>
73#include <visp3/vs/vpServo.h>
74#include <visp3/vs/vpServoDisplay.h>
75#include <visp3/gui/vpPlot.h>
76
77#if defined(VISP_HAVE_FLIR_PTU_SDK) && defined(VISP_HAVE_FLYCAPTURE) && \
78 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
79
80
81int main(int argc, char **argv)
82{
83 std::string opt_portname;
84 int opt_baudrate = 9600;
85 bool opt_network = false;
86 std::string opt_extrinsic;
87 double opt_tag_size = 0.120; // Used to compute the distance of the cog wrt the camera
88 double opt_constant_gain = 0.5;
89
90 if (argc == 1) {
91 std::cout << "To see how to use this example, run: " << argv[0] << " --help" << std::endl;
92 return EXIT_SUCCESS;
93 }
94
95 for (int i = 1; i < argc; i++) {
96 if ((std::string(argv[i]) == "--portname" || std::string(argv[i]) == "-p") && (i + 1 < argc)) {
97 opt_portname = std::string(argv[i + 1]);
98 }
99 else if ((std::string(argv[i]) == "--baudrate" || std::string(argv[i]) == "-b") && (i + 1 < argc)) {
100 opt_baudrate = std::atoi(argv[i + 1]);
101 }
102 else if ((std::string(argv[i]) == "--network" || std::string(argv[i]) == "-n")) {
103 opt_network = true;
104 }
105 else if (std::string(argv[i]) == "--extrinsic" && i + 1 < argc) {
106 opt_extrinsic = std::string(argv[i + 1]);
107 }
108 else if (std::string(argv[i]) == "--constant-gain" || std::string(argv[i]) == "-g") {
109 opt_constant_gain = std::stod(argv[i + 1]);;
110 }
111 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
112 std::cout << "SYNOPSIS" << std::endl
113 << " " << argv[0] << " [--portname <portname>] [--baudrate <rate>] [--network] "
114 << "[--extrinsic <extrinsic.yaml>] [--constant-gain] [--help] [-h]" << std::endl << std::endl;
115 std::cout << "DESCRIPTION" << std::endl
116 << " --portname, -p <portname>" << std::endl
117 << " Set serial or tcp port name." << std::endl << std::endl
118 << " --baudrate, -b <rate>" << std::endl
119 << " Set serial communication baud rate. Default: " << opt_baudrate << "." << std::endl << std::endl
120 << " --network, -n" << std::endl
121 << " Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl << std::endl
122 << " --extrinsic <extrinsic.yaml>" << std::endl
123 << " YAML file containing extrinsic camera parameters as a vpHomogeneousMatrix." << std::endl
124 << " It corresponds to the homogeneous transformation eMc, between end-effector" << std::endl
125 << " and camera frame." << std::endl << std::endl
126 << " --constant-gain, -g" << std::endl
127 << " Constant gain value. Default value: " << opt_constant_gain << std::endl << std::endl
128 << " --help, -h" << std::endl
129 << " Print this helper message. " << std::endl << std::endl;
130 std::cout << "EXAMPLE" << std::endl
131 << " - How to get network IP" << std::endl
132 #ifdef _WIN32
133 << " $ " << argv[0] << " --portname COM1 --network" << std::endl
134 << " Try to connect FLIR PTU to port: COM1 with baudrate: 9600" << std::endl
135 #else
136 << " $ " << argv[0] << " --portname /dev/ttyUSB0 --network" << std::endl
137 << " Try to connect FLIR PTU to port: /dev/ttyUSB0 with baudrate: 9600" << std::endl
138 #endif
139 << " PTU HostName: PTU-5" << std::endl
140 << " PTU IP : 169.254.110.254" << std::endl
141 << " PTU Gateway : 0.0.0.0" << std::endl
142 << " - How to run this binary using network communication" << std::endl
143 << " $ " << argv[0] << " --portname tcp:169.254.110.254 --tag-size 0.1 --gain 0.1" << std::endl;
144
145 return EXIT_SUCCESS;
146 }
147 }
148
149 vpRobotFlirPtu robot;
150
151 try {
152 std::cout << "Try to connect FLIR PTU to port: " << opt_portname << " with baudrate: " << opt_baudrate << std::endl;
153 robot.connect(opt_portname, opt_baudrate);
154
155 if(opt_network) {
156 std::cout << "PTU HostName: " << robot.getNetworkHostName() <<std::endl;
157 std::cout << "PTU IP : " << robot.getNetworkIP() <<std::endl;
158 std::cout << "PTU Gateway : " << robot.getNetworkGateway() <<std::endl;
159 return EXIT_SUCCESS;
160 }
161
163
165 g.open(I);
166
167 // Get camera extrinsics
170 eRc << 0, 0, 1,
171 -1, 0, 0,
172 0, -1, 0;
173 etc << -0.1, -0.123, 0.035;
174 vpHomogeneousMatrix eMc(etc, eRc);
175
176 if (!opt_extrinsic.empty()) {
177 vpPoseVector ePc;
178 ePc.loadYAML(opt_extrinsic, ePc);
179 eMc.buildFrom(ePc);
180 }
181
182 std::cout << "Considered extrinsic transformation eMc:\n" << eMc << std::endl;
183
184 // Get camera intrinsics
185 vpCameraParameters cam(900, 900, I.getWidth() / 2., I.getHeight() / 2.);
186 std::cout << "Considered intrinsic camera parameters:\n" << cam << "\n";
187
188#if defined(VISP_HAVE_X11)
189 vpDisplayX dc(I, 10, 10, "Color image");
190#elif defined(VISP_HAVE_GDI)
191 vpDisplayGDI dc(I, 10, 10, "Color image");
192#endif
193
195 detector.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
196 detector.setDisplayTag(true);
197 detector.setAprilTagQuadDecimate(2);
198
199 // Create visual features
200 vpFeaturePoint p, pd; // We use 1 point, the tag cog
201
202 // Set desired position to the image center
203 pd.set_x(0);
204 pd.set_y(0);
205
206 vpServo task;
207 // Add the visual feature point
208 task.addFeature(p, pd);
211 task.setLambda(opt_constant_gain);
212
213 bool final_quit = false;
214 bool send_velocities = false;
215 vpMatrix eJe;
216
217 robot.set_eMc(eMc); // Set location of the camera wrt end-effector frame
218
219 vpVelocityTwistMatrix cVe = robot.get_cVe();
220 task.set_cVe(cVe);
221
223
224 std::vector<vpHomogeneousMatrix> cMo_vec;
225 vpColVector qdot(2);
226
227 while (!final_quit) {
228 g.acquire(I);
229
231
232 detector.detect(I, opt_tag_size, cam, cMo_vec);
233
234 std::stringstream ss;
235 ss << "Left click to " << (send_velocities ? "stop the robot" : "servo the robot") << ", right click to quit.";
236 vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red);
237
238 // Only one tag has to be detected
239 if (detector.getNbObjects() == 1) {
240
241 vpImagePoint cog = detector.getCog(0);
242 double Z = cMo_vec[0][2][3];
243
244 // Update current feature from measured cog position
245 double x = 0, y = 0;
247 p.set_xyZ(x, y, Z);
248 pd.set_Z(Z);
249
250 // Get robot Jacobian
251 robot.get_eJe(eJe);
252 task.set_eJe(eJe);
253
254 qdot = task.computeControlLaw();
255
256 // Display the current and desired feature points in the image display
257 vpServoDisplay::display(task, cam, I);
258 } // end if (cMo_vec.size() == 1)
259 else {
260 qdot = 0;
261 }
262
263 if (!send_velocities) {
264 qdot = 0;
265 }
266
267 // Send to the robot
269
271
273 if (vpDisplay::getClick(I, button, false)) {
274 switch (button) {
276 send_velocities = !send_velocities;
277 break;
278
280 final_quit = true;
281 qdot = 0;
282 break;
283
284 default:
285 break;
286 }
287 }
288 }
289 std::cout << "Stop the robot " << std::endl;
291 }
292 catch (const vpRobotException & e) {
293 std::cout << "Catch Flir Ptu exception: " << e.getMessage() << std::endl;
295 }
296
297 return EXIT_SUCCESS;
298}
299#else
300int main()
301{
302#if !defined(VISP_HAVE_FLYCAPTURE)
303 std::cout << "Install FLIR Flycapture" << std::endl;
304#endif
305#if !defined(VISP_HAVE_FLIR_PTU_SDK)
306 std::cout << "Install FLIR PTU SDK." << std::endl;
307#endif
308 return 0;
309}
310#endif
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=NULL)
Definition: vpArray2D.h:652
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static const vpColor red
Definition: vpColor.h:217
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
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)
const char * getMessage() const
Definition: vpException.cpp:90
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void set_xyZ(double x, double y, double Z)
void set_y(double y)
void set_x(double x)
void set_Z(double Z)
void open(vpImage< unsigned char > &I)
void acquire(vpImage< unsigned char > &I)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
Error that can be emited by the vpRobot class and its derivates.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ JOINT_STATE
Definition: vpRobot.h:80
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
Implementation of a rotation matrix and operations on such kind of matrices.
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)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:159
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:448
void setLambda(double c)
Definition: vpServo.h:404
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
@ CURRENT
Definition: vpServo.h:182
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
Class that consider the case of a translation vector.
vpVelocityTwistMatrix get_cVe() const
Definition: vpUnicycle.h:82