Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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 
81 int 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
169  vpRotationMatrix eRc;
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 
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;
246  vpPixelMeterConversion::convertPoint(cam, cog, x, y);
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
268  robot.setVelocity(vpRobot::JOINT_STATE, qdot);
269 
270  vpDisplay::flush(I);
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
300 int 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
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
Error that can be emited by the vpRobot class and its derivates.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=NULL)
Definition: vpArray2D.h:652
Implementation of an homogeneous matrix and operations on such kind of matrices.
AprilTag 36h11 pattern (recommended)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:134
void acquire(vpImage< unsigned char > &I)
std::string getNetworkHostName()
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
size_t getNbObjects() const
const char * getMessage() const
Definition: vpException.cpp:90
void set_y(double y)
void set_x(double x)
static void flush(const vpImage< unsigned char > &I)
void open(vpImage< unsigned char > &I)
static const vpColor red
Definition: vpColor.h:217
Implementation of a rotation matrix and operations on such kind of matrices.
void setAprilTagQuadDecimate(float quadDecimate)
std::string getNetworkGateway()
Initialize the velocity controller.
Definition: vpRobot.h:66
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void set_Z(double Z)
void set_eMc(vpHomogeneousMatrix &eMc)
static void display(const vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:404
void set_xyZ(double x, double y, double Z)
void get_eJe(vpMatrix &eJe)
vpImagePoint getCog(size_t i) const
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
void connect(const std::string &portname, int baudrate=9600)
unsigned int getHeight() const
Definition: vpImage.h:188
std::string getNetworkIP()
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setDisplayTag(bool display, const vpColor &color=vpColor::none, unsigned int thickness=2)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:448
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
vpVelocityTwistMatrix get_cVe() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
unsigned int getWidth() const
Definition: vpImage.h:246
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
Class that consider the case of a translation vector.
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)
bool detect(const vpImage< unsigned char > &I)