Visual Servoing Platform version 3.5.0
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
53int 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 }
69 else if ((std::string(argv[i]) == "--baudrate" || std::string(argv[i]) == "-b") && (i + 1 < argc)) {
70 opt_baudrate = std::atoi(argv[i + 1]);
71 }
72 else if ((std::string(argv[i]) == "--network" || std::string(argv[i]) == "-n")) {
73 opt_network = true;
74 }
75 else if ((std::string(argv[i]) == "--reset" || std::string(argv[i]) == "-r")) {
76 opt_reset = true;
77 }
78 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
79 std::cout << "SYNOPSIS" << std::endl
80 << " " << argv[0] << " [--portname <portname>] [--baudrate <rate>] [--network] [--reset] [--help] [-h]" << std::endl << std::endl
81 << "DESCRIPTION" << std::endl
82 << " --portname, -p <portname>" << std::endl
83 << " Set serial or tcp port name." << std::endl << std::endl
84 << " --baudrate, -b <rate>" << std::endl
85 << " Set serial communication baud rate. Default: " << opt_baudrate << "." << std::endl << std::endl
86 << " --network, -n" << std::endl
87 << " Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl << std::endl
88 << " --reset, -r" << std::endl
89 << " Reset PTU axis and exit. " << std::endl << std::endl
90 << " --help, -h" << std::endl
91 << " Print this helper message. " << std::endl << 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]) << " " << vpMath::deg(robot.getPanPosLimits()[1]) << std::endl;
147 std::cout << "Tilt pos min/max [deg]: " << vpMath::deg(robot.getTiltPosLimits()[0]) << " " << vpMath::deg(robot.getTiltPosLimits()[1]) << std::endl;
148 std::cout << "Pan/tilt vel max [deg/s]: " << vpMath::deg(robot.getPanTiltVelMax()[0]) << " " << vpMath::deg(robot.getPanTiltVelMax()[1]) << std::endl << std::endl;
149 }
150
151 {
152 std::cout << "** Test limits setter" << std::endl;
153 // Reduce pan/tilt position limits wrt factory settings
154 vpColVector pan_pos_limits(2), tilt_pos_limits(2);
155 pan_pos_limits[0] = vpMath::rad(-90);
156 pan_pos_limits[1] = vpMath::rad(90);
157 tilt_pos_limits[0] = vpMath::rad(-20);
158 tilt_pos_limits[1] = vpMath::rad(20);
159
160 robot.setPanPosLimits(pan_pos_limits);
161 robot.setTiltPosLimits(tilt_pos_limits);
162
163 std::cout << "Modified user min/max limits: " << std::endl;
164 std::cout << "Pan pos min/max [deg]: " << vpMath::deg(robot.getPanPosLimits()[0]) << " " << vpMath::deg(robot.getPanPosLimits()[1]) << std::endl;
165 std::cout << "Tilt pos min/max [deg]: " << vpMath::deg(robot.getTiltPosLimits()[0]) << " " << vpMath::deg(robot.getTiltPosLimits()[1]) << std::endl;
166 std::cout << "Pan/tilt vel max [deg/s]: " << vpMath::deg(robot.getPanTiltVelMax()[0]) << " " << vpMath::deg(robot.getPanTiltVelMax()[1]) << std::endl << std::endl;
167 }
168
169 {
170 std::cout << "** Test position getter" << std::endl;
171 robot.getPosition(vpRobot::ARTICULAR_FRAME, q_mes);
172 std::cout << "Current position [deg]: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << std::endl;
173
174 std::cout << "Initialisation done." << std::endl << std::endl;
175 }
176
177 {
178 std::cout << "** Test joint positioning" << std::endl;
180 robot.setMaxRotationVelocity(std::min(robot.getPanTiltVelMax()[0], robot.getPanTiltVelMax()[1]) / 2.); // 50% of the slowest axis
181
182 q = 0;
183 std::cout << "Set joint position [deg]: " << vpMath::deg(q[0]) << " " << vpMath::deg(q[1]) << std::endl;
184 std::cout << "Enter a caracter to apply" << std::endl;
185 scanf("%d", &answer);
186
187 robot.setPositioningVelocity(50);
188 robot.setPosition(vpRobot::JOINT_STATE, q);
189 robot.getPosition(vpRobot::JOINT_STATE, q_mes);
190
191 std::cout << "Position reached [deg]: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << std::endl << std::endl;
192 }
193
194 {
195 std::cout << "** Test joint positioning" << std::endl;
196 q[0] = vpMath::rad(10); // Pan position in rad
197 q[1] = vpMath::rad(20); // Tilt position in rad
198
199 std::cout << "Set joint position: " << vpMath::deg(q[0]) << " " << vpMath::deg(q[1]) << "[deg]" << std::endl;
200 std::cout << "Enter a caracter to apply" << std::endl;
201 scanf("%d", &answer);
202
203 robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
204 robot.getPosition(vpRobot::ARTICULAR_FRAME, q_mes);
205
206 std::cout << "Position reached [deg]: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << std::endl << std::endl;
207 }
208
209 {
210 std::cout << "** Test joint velocity" << std::endl;
211
212 vpColVector qdot(2);
213 qdot[0] = vpMath::rad(-10); // Pan velocity in rad/s
214 qdot[1] = vpMath::rad(0); // Tilt velocity in rad/s
215
216 std::cout << "Set velocity for 4s: " << vpMath::deg(qdot[0]) << " " << vpMath::deg(qdot[1]) << " [deg/s]" << std::endl;
217 std::cout << "Enter a caracter to apply" << std::endl;
218 scanf("%d", &answer);
219
221
222 double t_start = vpTime::measureTimeMs();
223 do {
225 vpTime::sleepMs(40);
226 } while (vpTime::measureTimeMs() - t_start < 4000);
227
229 robot.getPosition(vpRobot::ARTICULAR_FRAME, q_mes);
230 std::cout << "Position reached: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << " [deg]" << std::endl << std::endl;
231 }
232
233 {
234 std::cout << "** Test cartesian velocity with robot Jacobien eJe" << std::endl;
235
236 vpColVector v_e(6, 0);
237 v_e[4] = vpMath::rad(5); //wy_e
238 v_e[5] = vpMath::rad(5); //wz_e
239
240 std::cout << "Set cartesian velocity in end-effector frame for 4s: " << v_e[0] << " " << v_e[1] << " " << v_e[2] << " [m/s] "
241 << vpMath::deg(v_e[3]) << " " << vpMath::deg(v_e[4]) << " " << vpMath::deg(v_e[5]) << " [deg/s]" << std::endl;
242 std::cout << "Enter a caracter to apply" << std::endl;
243 scanf("%d", &answer);
244
246
247 double t_start = vpTime::measureTimeMs();
248 do {
249 vpColVector qdot = robot.get_eJe().pseudoInverse() * v_e;
251 vpTime::sleepMs(40);
252 } while (vpTime::measureTimeMs() - t_start < 4000);
253
255 robot.getPosition(vpRobot::ARTICULAR_FRAME, q_mes);
256 std::cout << "Position reached: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << " [deg]" << std::endl << std::endl;
257 }
258
259 std::cout << "** The end" << std::endl;
260 } catch (const vpRobotException &e) {
261 std::cout << "Catch Flir Ptu signal exception: " << e.getMessage() << std::endl;
263 } catch (const vpException &e) {
264 std::cout << "Catch Flir Ptu exception: " << e.getMessage() << std::endl;
266 }
267 return EXIT_SUCCESS;
268}
269#else
270int main()
271{
272 std::cout << "You do not have an Flir Ptu robot connected to your computer..." << std::endl;
273 return EXIT_SUCCESS;
274}
275
276#endif
277
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:90
static double rad(double deg)
Definition: vpMath.h:110
static double deg(double rad)
Definition: vpMath.h:103
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:78
@ JOINT_STATE
Definition: vpRobot.h:80
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:67
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
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()