Visual Servoing Platform  version 3.6.1 under development (2025-02-17)
testForceTorqueAtiNetFTSensor.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Test force/torque ATI sensor.
32  */
33 
40 #include <iostream>
41 
42 #include <visp3/gui/vpPlot.h>
43 #include <visp3/sensor/vpForceTorqueAtiNetFTSensor.h>
44 
45 int main(int argc, char **argv)
46 {
47 #ifdef ENABLE_VISP_NAMESPACE
48  using namespace VISP_NAMESPACE_NAME;
49 #endif
50  // Check if vpForceTorqueAtiNetFTSensor can be used since inet_ntop() used to
51  // communicate by UDP with the sensor is not supported on win XP
52 #ifdef VISP_HAVE_FUNC_INET_NTOP
53 
54  std::string opt_ip = "192.168.1.1";
55  int opt_port = 49152;
56  bool opt_no_display = false;
57 
58  for (int i = 1; i < argc; i++) {
59  if (std::string(argv[i]) == "--ip" && i + 1 < argc) {
60  opt_ip = std::string(argv[++i]);
61  }
62  else if (std::string(argv[i]) == "--port" && i + 1 < argc) {
63  opt_port = atoi(argv[++i]);
64  }
65  else if (std::string(argv[i]) == "--no-display" || std::string(argv[i]) == "-d") {
66  opt_no_display = true;
67  }
68  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
69  std::cout << "\nUsage: " << argv[0]
70  << " [--ip <Net F/T IP address (default: 192.168.1.1)>]"
71  << " [--port <Ethernet port (default: 49152)>]"
72  << " [--no-display] [-d] [--help] [-h]\n"
73  << std::endl;
74  return EXIT_SUCCESS;
75  }
76  }
77 
78  std::cout << "Use IP : " << opt_ip << std::endl;
79  std::cout << "Use port: " << opt_port << std::endl;
80  std::cout << "Disable display: " << opt_no_display << std::endl;
81 
82  vpForceTorqueAtiNetFTSensor ati_net_ft;
83  ati_net_ft.init(opt_ip, opt_port);
84  if (!ati_net_ft.startStreaming()) {
85  std::cout << "Unable to start streaming" << std::endl;
86  return EXIT_FAILURE;
87  }
88 
89 #if defined(VISP_HAVE_DISPLAY)
90  vpPlot *plotter = nullptr;
91  if (!opt_no_display) {
92  plotter = new vpPlot(2, 700, 700, 100, 200, "Curves...");
93  plotter->initGraph(0, 3);
94  plotter->setTitle(0, "Force measurements");
95  plotter->setLegend(0, 0, "Fx");
96  plotter->setLegend(0, 1, "Fy");
97  plotter->setLegend(0, 2, "Fz");
98  plotter->initGraph(1, 3);
99  plotter->setTitle(1, "Torque measurements");
100  plotter->setLegend(1, 0, "Tx");
101  plotter->setLegend(1, 1, "Ty");
102  plotter->setLegend(1, 2, "Tz");
103  }
104  bool bias = false;
105 #endif
106 
107  vpColVector ft;
108  bool end = false;
109  unsigned long nbacq = 0;
110  double t_start = vpTime::measureTimeMs();
111  while (!end) {
112  double t = vpTime::measureTimeMs();
113 
114  if (ati_net_ft.waitForNewData()) {
115  ft = ati_net_ft.getForceTorque();
116 #if defined(VISP_HAVE_DISPLAY)
117  if (!opt_no_display) {
118  vpColVector force = ft.extract(0, 3);
119  vpColVector torque = ft.extract(3, 3);
120  plotter->plot(0, nbacq, force);
121  plotter->plot(1, nbacq, torque);
122  vpDisplay::displayText(plotter->I, 20, 80, "Left click to quit", vpColor::red);
123  if (bias) {
124  vpDisplay::displayText(plotter->I, 40, 80, "Right click to unbias", vpColor::red);
125  }
126  else {
127  vpDisplay::displayText(plotter->I, 40, 80, "Right click to bias", vpColor::red);
128  }
130 
131  if (vpDisplay::getClick(plotter->I, button, false)) {
132  if (button == vpMouseButton::button1) {
133  end = true;
134  }
135  else if (button == vpMouseButton::button3) {
136  bias = !bias;
137  if (bias) {
138  std::cout << "Bias F/T sensor" << std::endl;
139  ati_net_ft.bias();
140  }
141  else {
142  std::cout << "Unbias F/T sensor" << std::endl;
143  ati_net_ft.unbias();
144  }
145  }
146  }
147  vpDisplay::flush(plotter->I);
148  }
149  else {
150  std::cout << "F/T: " << ft.t() << std::endl;
151  if (nbacq > 30) {
152  end = true;
153  }
154  }
155 #else
156  std::cout << "F/T: " << ft.t() << std::endl;
157  if (nbacq > 30) {
158  end = true;
159  }
160 #endif
161  nbacq++;
162  }
163  vpTime::wait(t, 2);
164  std::cout << "Loop time: " << vpTime::measureTimeMs() - t << " ms" << std::endl;
165  }
166  ati_net_ft.stopStreaming();
167  double fps = 1000. * nbacq / (vpTime::measureTimeMs() - t_start);
168 
169 #if defined(VISP_HAVE_DISPLAY)
170  if (plotter) {
171  delete plotter;
172  }
173 #endif
174 
175  std::cout << "Mean acquisition frequency: " << fps << " Hz" << std::endl;
176  std::cout << "Test succeed" << std::endl;
177 #else
178  (void)argc;
179  (void)argv;
180  std::cout << "vpForceTorqueAtiNetFTSensor is not supported on this platform" << std::endl;
181 #endif
182  return EXIT_SUCCESS;
183 }
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:410
vpRowVector t() const
static const vpColor red
Definition: vpColor.h:198
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:112
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:203
vpImage< unsigned char > I
Definition: vpPlot.h:114
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:552
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:270
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:510
void init(const std::string &hostname, int port)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()