Visual Servoing Platform  version 3.6.1 under development (2024-06-12)
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/vpConfig.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 
77 int main(int argc, char **argv)
78 {
79 #ifdef ENABLE_VISP_NAMESPACE
80  using namespace VISP_NAMESPACE_NAME;
81 #endif
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  }
112  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
113  std::cout << "SYNOPSIS" << std::endl
114  << " " << argv[0] << " [--portname <portname>] [--baudrate <rate>] [--network] "
115  << "[--extrinsic <extrinsic.yaml>] [--constant-gain] [--help] [-h]" << std::endl
116  << std::endl;
117  std::cout << "DESCRIPTION" << std::endl
118  << " --portname, -p <portname>" << std::endl
119  << " Set serial or tcp port name." << std::endl
120  << std::endl
121  << " --baudrate, -b <rate>" << std::endl
122  << " Set serial communication baud rate. Default: " << opt_baudrate << "." << std::endl
123  << std::endl
124  << " --network, -n" << std::endl
125  << " Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl
126  << std::endl
127  << " --extrinsic <extrinsic.yaml>" << std::endl
128  << " YAML file containing extrinsic camera parameters as a vpHomogeneousMatrix." << std::endl
129  << " It corresponds to the homogeneous transformation eMc, between end-effector" << std::endl
130  << " and camera frame." << std::endl
131  << std::endl
132  << " --constant-gain, -g" << std::endl
133  << " Constant gain value. Default value: " << opt_constant_gain << std::endl
134  << std::endl
135  << " --help, -h" << std::endl
136  << " Print this helper message. " << std::endl
137  << std::endl;
138  std::cout << "EXAMPLE" << std::endl
139  << " - How to get network IP" << std::endl
140 #ifdef _WIN32
141  << " $ " << argv[0] << " --portname COM1 --network" << std::endl
142  << " Try to connect FLIR PTU to port: COM1 with baudrate: 9600" << std::endl
143 #else
144  << " $ " << argv[0] << " --portname /dev/ttyUSB0 --network" << std::endl
145  << " Try to connect FLIR PTU to port: /dev/ttyUSB0 with baudrate: 9600" << std::endl
146 #endif
147  << " PTU HostName: PTU-5" << std::endl
148  << " PTU IP : 169.254.110.254" << std::endl
149  << " PTU Gateway : 0.0.0.0" << std::endl
150  << " - How to run this binary using network communication" << std::endl
151  << " $ " << argv[0] << " --portname tcp:169.254.110.254 --tag-size 0.1 --gain 0.1" << std::endl;
152 
153  return EXIT_SUCCESS;
154  }
155  }
156 
157  vpRobotFlirPtu robot;
158 
159  try {
160  std::cout << "Try to connect FLIR PTU to port: " << opt_portname << " with baudrate: " << opt_baudrate << std::endl;
161  robot.connect(opt_portname, opt_baudrate);
162 
163  if (opt_network) {
164  std::cout << "PTU HostName: " << robot.getNetworkHostName() << std::endl;
165  std::cout << "PTU IP : " << robot.getNetworkIP() << std::endl;
166  std::cout << "PTU Gateway : " << robot.getNetworkGateway() << std::endl;
167  return EXIT_SUCCESS;
168  }
169 
171 
173  g.open(I);
174 
175  // Get camera extrinsics
177  vpRotationMatrix eRc;
178  eRc << 0, 0, 1, -1, 0, 0, 0, -1, 0;
179  etc << -0.1, -0.123, 0.035;
180  vpHomogeneousMatrix eMc(etc, eRc);
181 
182  if (!opt_extrinsic.empty()) {
183  vpPoseVector ePc;
184  ePc.loadYAML(opt_extrinsic, ePc);
185  eMc.build(ePc);
186  }
187 
188  std::cout << "Considered extrinsic transformation eMc:\n" << eMc << std::endl;
189 
190  // Get camera intrinsics
191  vpCameraParameters cam(900, 900, I.getWidth() / 2., I.getHeight() / 2.);
192  std::cout << "Considered intrinsic camera parameters:\n" << cam << "\n";
193 
194 #if defined(VISP_HAVE_X11)
195  vpDisplayX dc(I, 10, 10, "Color image");
196 #elif defined(VISP_HAVE_GDI)
197  vpDisplayGDI dc(I, 10, 10, "Color image");
198 #endif
199 
201  detector.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
202  detector.setDisplayTag(true);
203  detector.setAprilTagQuadDecimate(2);
204 
205  // Create visual features
206  vpFeaturePoint p, pd; // We use 1 point, the tag cog
207 
208  // Set desired position to the image center
209  pd.set_x(0);
210  pd.set_y(0);
211 
212  vpServo task;
213  // Add the visual feature point
214  task.addFeature(p, pd);
217  task.setLambda(opt_constant_gain);
218 
219  bool final_quit = false;
220  bool send_velocities = false;
221  vpMatrix eJe;
222 
223  robot.set_eMc(eMc); // Set location of the camera wrt end-effector frame
224 
225  vpVelocityTwistMatrix cVe = robot.get_cVe();
226  task.set_cVe(cVe);
227 
229 
230  std::vector<vpHomogeneousMatrix> cMo_vec;
231  vpColVector qdot(2);
232 
233  while (!final_quit) {
234  g.acquire(I);
235 
237 
238  detector.detect(I, opt_tag_size, cam, cMo_vec);
239 
240  std::stringstream ss;
241  ss << "Left click to " << (send_velocities ? "stop the robot" : "servo the robot") << ", right click to quit.";
242  vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red);
243 
244  // Only one tag has to be detected
245  if (detector.getNbObjects() == 1) {
246 
247  vpImagePoint cog = detector.getCog(0);
248  double Z = cMo_vec[0][2][3];
249 
250  // Update current feature from measured cog position
251  double x = 0, y = 0;
252  vpPixelMeterConversion::convertPoint(cam, cog, x, y);
253  p.set_xyZ(x, y, Z);
254  pd.set_Z(Z);
255 
256  // Get robot Jacobian
257  robot.get_eJe(eJe);
258  task.set_eJe(eJe);
259 
260  qdot = task.computeControlLaw();
261 
262  // Display the current and desired feature points in the image display
263  vpServoDisplay::display(task, cam, I);
264  } // end if (cMo_vec.size() == 1)
265  else {
266  qdot = 0;
267  }
268 
269  if (!send_velocities) {
270  qdot = 0;
271  }
272 
273  // Send to the robot
274  robot.setVelocity(vpRobot::JOINT_STATE, qdot);
275 
276  vpDisplay::flush(I);
277 
279  if (vpDisplay::getClick(I, button, false)) {
280  switch (button) {
282  send_velocities = !send_velocities;
283  break;
284 
286  final_quit = true;
287  qdot = 0;
288  break;
289 
290  default:
291  break;
292  }
293  }
294  }
295  std::cout << "Stop the robot " << std::endl;
297  }
298  catch (const vpRobotException &e) {
299  std::cout << "Catch Flir Ptu exception: " << e.getMessage() << std::endl;
301  }
302 
303  return EXIT_SUCCESS;
304 }
305 #else
306 int main()
307 {
308 #if !defined(VISP_HAVE_FLYCAPTURE)
309  std::cout << "Install FLIR Flycapture" << std::endl;
310 #endif
311 #if !defined(VISP_HAVE_FLIR_PTU_SDK)
312  std::cout << "Install FLIR PTU SDK." << std::endl;
313 #endif
314  return EXIT_SUCCESS;
315 }
316 #endif
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=nullptr)
Definition: vpArray2D.h:773
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:171
static const vpColor red
Definition: vpColor.h:213
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:131
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:65
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:82
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:185
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:151
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:187
Error that can be emitted by the vpRobot class and its derivatives.
void get_eJe(vpMatrix &eJe) vp_override
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
@ JOINT_STATE
Definition: vpRobot.h:82
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:66
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:380
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:164
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:331
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:1030
void setLambda(double c)
Definition: vpServo.h:978
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:1093
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:134
vpColVector computeControlLaw()
Definition: vpServo.cpp:705
@ CURRENT
Definition: vpServo.h:198
Class that consider the case of a translation vector.
vpVelocityTwistMatrix get_cVe() const
Definition: vpUnicycle.h:72