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