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