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