Visual Servoing Platform  version 3.5.1 under development (2022-07-06)
testRobotFlirPtu.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  * Test FLIR PTU interface.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
45 #include <iostream>
46 
47 #include <visp3/core/vpConfig.h>
48 
49 #ifdef VISP_HAVE_FLIR_PTU_SDK
50 
51 #include <visp3/robot/vpRobotFlirPtu.h>
52 
53 int main(int argc, char **argv)
54 {
55  std::string opt_portname;
56  int opt_baudrate = 9600;
57  bool opt_network = false;
58  bool opt_reset = false;
59 
60  if (argc == 1) {
61  std::cout << "To see how to use this test, run: " << argv[0] << " --help" << std::endl;
62  return EXIT_SUCCESS;
63  }
64 
65  for (int i = 1; i < argc; i++) {
66  if ((std::string(argv[i]) == "--portname" || std::string(argv[i]) == "-p") && (i + 1 < argc)) {
67  opt_portname = std::string(argv[i + 1]);
68  } else if ((std::string(argv[i]) == "--baudrate" || std::string(argv[i]) == "-b") && (i + 1 < argc)) {
69  opt_baudrate = std::atoi(argv[i + 1]);
70  } else if ((std::string(argv[i]) == "--network" || std::string(argv[i]) == "-n")) {
71  opt_network = true;
72  } else if ((std::string(argv[i]) == "--reset" || std::string(argv[i]) == "-r")) {
73  opt_reset = true;
74  } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
75  std::cout << "SYNOPSIS" << std::endl
76  << " " << argv[0] << " [--portname <portname>] [--baudrate <rate>] [--network] [--reset] [--help] [-h]"
77  << std::endl
78  << std::endl
79  << "DESCRIPTION" << std::endl
80  << " --portname, -p <portname>" << std::endl
81  << " Set serial or tcp port name." << std::endl
82  << std::endl
83  << " --baudrate, -b <rate>" << std::endl
84  << " Set serial communication baud rate. Default: " << opt_baudrate << "." << std::endl
85  << std::endl
86  << " --network, -n" << std::endl
87  << " Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl
88  << std::endl
89  << " --reset, -r" << std::endl
90  << " Reset PTU axis and exit. " << std::endl
91  << std::endl
92  << " --help, -h" << std::endl
93  << " Print this helper message. " << std::endl
94  << std::endl
95  << "EXAMPLE" << std::endl
96  << " - How to get network IP" << std::endl
97 #ifdef _WIN32
98  << " $ " << argv[0] << " -p /dev/ttyUSB0 --network" << std::endl
99 #else
100  << " $ " << argv[0] << " --portname COM1 --network" << std::endl
101 #endif
102  << " Try to connect FLIR PTU to port: /dev/ttyUSB0 with baudrate: 9600" << std::endl
103  << " PTU HostName: PTU-5" << std::endl
104  << " PTU IP : 169.254.110.254" << std::endl
105  << " PTU Gateway : 0.0.0.0" << std::endl
106  << " - How to run this binary using serial communication" << std::endl
107 #ifdef _WIN32
108  << " $ " << argv[0] << " --portname COM1" << std::endl
109 #else
110  << " $ " << argv[0] << " --portname /dev/ttyUSB0" << std::endl
111 #endif
112  << " - How to run this binary using network communication" << std::endl
113  << " $ " << argv[0] << " --portname tcp:169.254.110.254" << std::endl;
114 
115  return EXIT_SUCCESS;
116  }
117  }
118 
119  if (opt_portname.empty()) {
120  std::cout << "Error, portname unspecified. Run " << argv[0] << " --help" << std::endl;
121  return EXIT_SUCCESS;
122  }
123 
124  vpRobotFlirPtu robot;
125 
126  try {
127  vpColVector q(2), q_mes;
128  int answer;
129 
130  std::cout << "Try to connect FLIR PTU to port: " << opt_portname << " with baudrate: " << opt_baudrate << std::endl;
131  robot.connect(opt_portname, opt_baudrate);
132 
133  if (opt_network) {
134  std::cout << "PTU HostName: " << robot.getNetworkHostName() << std::endl;
135  std::cout << "PTU IP : " << robot.getNetworkIP() << std::endl;
136  std::cout << "PTU Gateway : " << robot.getNetworkGateway() << std::endl;
137  return EXIT_SUCCESS;
138  }
139 
140  if (opt_reset) {
141  std::cout << "Reset PTU axis" << std::endl;
142  robot.reset();
143  return EXIT_SUCCESS;
144  }
145 
146  {
147  std::cout << "** Test limits getter" << std::endl;
148 
149  std::cout << "Pan pos min/max [deg]: " << vpMath::deg(robot.getPanPosLimits()[0]) << " "
150  << vpMath::deg(robot.getPanPosLimits()[1]) << std::endl;
151  std::cout << "Tilt pos min/max [deg]: " << vpMath::deg(robot.getTiltPosLimits()[0]) << " "
152  << vpMath::deg(robot.getTiltPosLimits()[1]) << std::endl;
153  std::cout << "Pan/tilt vel max [deg/s]: " << vpMath::deg(robot.getPanTiltVelMax()[0]) << " "
154  << vpMath::deg(robot.getPanTiltVelMax()[1]) << std::endl
155  << std::endl;
156  }
157 
158  {
159  std::cout << "** Test limits setter" << std::endl;
160  // Reduce pan/tilt position limits wrt factory settings
161  vpColVector pan_pos_limits(2), tilt_pos_limits(2);
162  pan_pos_limits[0] = vpMath::rad(-90);
163  pan_pos_limits[1] = vpMath::rad(90);
164  tilt_pos_limits[0] = vpMath::rad(-20);
165  tilt_pos_limits[1] = vpMath::rad(20);
166 
167  robot.setPanPosLimits(pan_pos_limits);
168  robot.setTiltPosLimits(tilt_pos_limits);
169 
170  std::cout << "Modified user min/max limits: " << std::endl;
171  std::cout << "Pan pos min/max [deg]: " << vpMath::deg(robot.getPanPosLimits()[0]) << " "
172  << vpMath::deg(robot.getPanPosLimits()[1]) << std::endl;
173  std::cout << "Tilt pos min/max [deg]: " << vpMath::deg(robot.getTiltPosLimits()[0]) << " "
174  << vpMath::deg(robot.getTiltPosLimits()[1]) << std::endl;
175  std::cout << "Pan/tilt vel max [deg/s]: " << vpMath::deg(robot.getPanTiltVelMax()[0]) << " "
176  << vpMath::deg(robot.getPanTiltVelMax()[1]) << std::endl
177  << std::endl;
178  }
179 
180  {
181  std::cout << "** Test position getter" << std::endl;
182  robot.getPosition(vpRobot::ARTICULAR_FRAME, q_mes);
183  std::cout << "Current position [deg]: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << std::endl;
184 
185  std::cout << "Initialisation done." << std::endl << std::endl;
186  }
187 
188  {
189  std::cout << "** Test joint positioning" << std::endl;
191  robot.setMaxRotationVelocity(std::min(robot.getPanTiltVelMax()[0], robot.getPanTiltVelMax()[1]) /
192  2.); // 50% of the slowest axis
193 
194  q = 0;
195  std::cout << "Set joint position [deg]: " << vpMath::deg(q[0]) << " " << vpMath::deg(q[1]) << std::endl;
196  std::cout << "Enter a caracter to apply" << std::endl;
197  scanf("%d", &answer);
198 
199  robot.setPositioningVelocity(50);
200  robot.setPosition(vpRobot::JOINT_STATE, q);
201  robot.getPosition(vpRobot::JOINT_STATE, q_mes);
202 
203  std::cout << "Position reached [deg]: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << std::endl
204  << std::endl;
205  }
206 
207  {
208  std::cout << "** Test joint positioning" << std::endl;
209  q[0] = vpMath::rad(10); // Pan position in rad
210  q[1] = vpMath::rad(20); // Tilt position in rad
211 
212  std::cout << "Set joint position: " << vpMath::deg(q[0]) << " " << vpMath::deg(q[1]) << "[deg]" << std::endl;
213  std::cout << "Enter a caracter to apply" << std::endl;
214  scanf("%d", &answer);
215 
216  robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
217  robot.getPosition(vpRobot::ARTICULAR_FRAME, q_mes);
218 
219  std::cout << "Position reached [deg]: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << std::endl
220  << std::endl;
221  }
222 
223  {
224  std::cout << "** Test joint velocity" << std::endl;
225 
226  vpColVector qdot(2);
227  qdot[0] = vpMath::rad(-10); // Pan velocity in rad/s
228  qdot[1] = vpMath::rad(0); // Tilt velocity in rad/s
229 
230  std::cout << "Set velocity for 4s: " << vpMath::deg(qdot[0]) << " " << vpMath::deg(qdot[1]) << " [deg/s]"
231  << std::endl;
232  std::cout << "Enter a caracter to apply" << std::endl;
233  scanf("%d", &answer);
234 
236 
237  double t_start = vpTime::measureTimeMs();
238  do {
239  robot.setVelocity(vpRobot::JOINT_STATE, qdot);
240  vpTime::sleepMs(40);
241  } while (vpTime::measureTimeMs() - t_start < 4000);
242 
244  robot.getPosition(vpRobot::ARTICULAR_FRAME, q_mes);
245  std::cout << "Position reached: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << " [deg]"
246  << std::endl
247  << std::endl;
248  }
249 
250  {
251  std::cout << "** Test cartesian velocity with robot Jacobien eJe" << std::endl;
252 
253  vpColVector v_e(6, 0);
254  v_e[4] = vpMath::rad(5); // wy_e
255  v_e[5] = vpMath::rad(5); // wz_e
256 
257  std::cout << "Set cartesian velocity in end-effector frame for 4s: " << v_e[0] << " " << v_e[1] << " " << v_e[2]
258  << " [m/s] " << vpMath::deg(v_e[3]) << " " << vpMath::deg(v_e[4]) << " " << vpMath::deg(v_e[5])
259  << " [deg/s]" << std::endl;
260  std::cout << "Enter a caracter to apply" << std::endl;
261  scanf("%d", &answer);
262 
264 
265  double t_start = vpTime::measureTimeMs();
266  do {
267  vpColVector qdot = robot.get_eJe().pseudoInverse() * v_e;
268  robot.setVelocity(vpRobot::JOINT_STATE, qdot);
269  vpTime::sleepMs(40);
270  } while (vpTime::measureTimeMs() - t_start < 4000);
271 
273  robot.getPosition(vpRobot::ARTICULAR_FRAME, q_mes);
274  std::cout << "Position reached: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << " [deg]"
275  << std::endl
276  << std::endl;
277  }
278 
279  std::cout << "** The end" << std::endl;
280  } catch (const vpRobotException &e) {
281  std::cout << "Catch Flir Ptu signal exception: " << e.getMessage() << std::endl;
283  } catch (const vpException &e) {
284  std::cout << "Catch Flir Ptu exception: " << e.getMessage() << std::endl;
286  }
287  return EXIT_SUCCESS;
288 }
289 #else
290 int main()
291 {
292  std::cout << "You do not have an Flir Ptu robot connected to your computer..." << std::endl;
293  return EXIT_SUCCESS;
294 }
295 
296 #endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
const char * getMessage() const
Definition: vpException.cpp:87
static double rad(double deg)
Definition: vpMath.h:117
static double deg(double rad)
Definition: vpMath.h:110
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)
@ ARTICULAR_FRAME
Definition: vpRobot.h:79
@ JOINT_STATE
Definition: vpRobot.h:81
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:68
@ 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:201
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:260
VISP_EXPORT void sleepMs(double t)
VISP_EXPORT double measureTimeMs()