Visual Servoing Platform version 3.5.0
testForceTorqueAtiNetFTSensor.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 force/torque ATI sensor.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
45#include <iostream>
46
47#include <visp3/gui/vpPlot.h>
48#include <visp3/sensor/vpForceTorqueAtiNetFTSensor.h>
49
50int main(int argc, char **argv)
51{
52 // Check if vpForceTorqueAtiNetFTSensor can be used since inet_ntop() used to
53 // communicate by UDP with the sensor is not supported on win XP
54#ifdef VISP_HAVE_FUNC_INET_NTOP
55
56 std::string opt_ip = "192.168.1.1";
57 int opt_port = 49152;
58 bool opt_no_display = false;
59
60 for (int i = 0; i < argc; i++) {
61 if (std::string(argv[i]) == "--ip")
62 opt_ip = std::string(argv[i + 1]);
63 else if (std::string(argv[i]) == "--port")
64 opt_port = atoi(argv[i + 1]);
65 else if (std::string(argv[i]) == "--no-display" || std::string(argv[i]) == "-d")
66 opt_no_display = true;
67 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
68 std::cout << "\nUsage: " << argv[0]
69 << " [--ip <Net F/T IP address (default: 192.168.1.1)>] [--port <Ethernet port (default: 49152)>]"
70 << " [--no-display] [-d] [--help] [-h]\n"
71 << std::endl;
72 return 0;
73 }
74 }
75
76 std::cout << "Use IP : " << opt_ip << std::endl;
77 std::cout << "Use port: " << opt_port << std::endl;
78 std::cout << "Disable display: " << opt_no_display << std::endl;
79
81 ati_net_ft.init(opt_ip, opt_port);
82 if (!ati_net_ft.startStreaming()) {
83 std::cout << "Unable to start streaming" << std::endl;
84 return EXIT_FAILURE;
85 }
86
87#if defined(VISP_HAVE_DISPLAY)
88 vpPlot *plotter = NULL;
89 if (!opt_no_display) {
90 plotter = new vpPlot(2, 700, 700, 100, 200, "Curves...");
91 plotter->initGraph(0, 3);
92 plotter->setTitle(0, "Force measurements");
93 plotter->setLegend(0, 0, "Fx");
94 plotter->setLegend(0, 1, "Fy");
95 plotter->setLegend(0, 2, "Fz");
96 plotter->initGraph(1, 3);
97 plotter->setTitle(1, "Torque measurements");
98 plotter->setLegend(1, 0, "Tx");
99 plotter->setLegend(1, 1, "Ty");
100 plotter->setLegend(1, 2, "Tz");
101 }
102 bool bias = false;
103#endif
104
105 vpColVector ft;
106 bool end = false;
107 unsigned long nbacq = 0;
108 double t_start = vpTime::measureTimeMs();
109 while (!end) {
110 double t = vpTime::measureTimeMs();
111
112 if (ati_net_ft.waitForNewData()) {
113 ft = ati_net_ft.getForceTorque();
114#if defined(VISP_HAVE_DISPLAY)
115 if (!opt_no_display) {
116 vpColVector force = ft.extract(0, 3);
117 vpColVector torque = ft.extract(3, 3);
118 plotter->plot(0, nbacq, force);
119 plotter->plot(1, nbacq, torque);
120 vpDisplay::displayText(plotter->I, 20, 80, "Left click to quit", vpColor::red);
121 if (bias) {
122 vpDisplay::displayText(plotter->I, 40, 80, "Right click to unbias", vpColor::red);
123 } else {
124 vpDisplay::displayText(plotter->I, 40, 80, "Right click to bias", vpColor::red);
125 }
127
128 if (vpDisplay::getClick(plotter->I, button, false)) {
129 if (button == vpMouseButton::button1) {
130 end = true;
131 } else if (button == vpMouseButton::button3) {
132 bias = !bias;
133 if (bias) {
134 std::cout << "Bias F/T sensor" << std::endl;
135 ati_net_ft.bias();
136 } else {
137 std::cout << "Unbias F/T sensor" << std::endl;
138 ati_net_ft.unbias();
139 }
140 }
141 }
142 vpDisplay::flush(plotter->I);
143 } else {
144 std::cout << "F/T: " << ft.t() << std::endl;
145 if (nbacq > 30) {
146 end = true;
147 }
148 }
149#else
150 std::cout << "F/T: " << ft.t() << std::endl;
151 if (nbacq > 30) {
152 end = true;
153 }
154#endif
155 nbacq++;
156 }
157 vpTime::wait(t, 2);
158 std::cout << "Loop time: " << vpTime::measureTimeMs() - t << " ms" << std::endl;
159 }
160 ati_net_ft.stopStreaming();
161 double fps = 1000. * nbacq / (vpTime::measureTimeMs() - t_start);
162
163#if defined(VISP_HAVE_DISPLAY)
164 if (plotter) {
165 delete plotter;
166 }
167#endif
168
169 std::cout << "Mean acquisition frequency: " << fps << " Hz" << std::endl;
170 std::cout << "Test succeed" << std::endl;
171#else
172 (void)argc;
173 (void)argv;
174 std::cout << "vpForceTorqueAtiNetFTSensor is not supported on this platform" << std::endl;
175#endif
176 return EXIT_SUCCESS;
177}
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:220
vpRowVector t() const
static const vpColor red
Definition: vpColor.h:217
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
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)
void bias(unsigned int n_counts=50)
bool waitForNewData(unsigned int timeout=50)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:116
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:206
vpImage< unsigned char > I
Definition: vpPlot.h:118
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:547
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:286
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:498
void init(const std::string &hostname, int port)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()