Visual Servoing Platform version 3.5.0
testForceTorqueIitSensor.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 IIT sensor.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
45#include <iostream>
46
47#include <visp3/gui/vpPlot.h>
48#include <visp3/sensor/vpForceTorqueIitSensor.h>
49
50int main(int argc, char **argv)
51{
52#ifdef VISP_HAVE_FT_IIT_SDK
53 bool opt_no_display = false;
54 bool opt_filtered = false;
55
56 for (int i = 0; i < argc; i++) {
57 if (std::string(argv[i]) == "--no-display" || std::string(argv[i]) == "-d")
58 opt_no_display = true;
59 if (std::string(argv[i]) == "--filtered" || std::string(argv[i]) == "-f")
60 opt_filtered = true;
61 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
62 std::cout << "\nUsage: " << argv[0] << " [--no-display] [--filtered] [--help] [-d] [-f] [-h]\n" << std::endl;
63 return 0;
64 }
65 }
66
67 std::cout << "Disable display: " << opt_no_display << std::endl;
68
70
71#if defined(VISP_HAVE_DISPLAY)
72 vpPlot *plotter = NULL;
73 if (!opt_no_display) {
74 plotter = new vpPlot(2, 700, 700, 100, 200, "Curves...");
75 plotter->initGraph(0, 3);
76 plotter->setTitle(0, "Force measurements");
77 plotter->setLegend(0, 0, "Fx");
78 plotter->setLegend(0, 1, "Fy");
79 plotter->setLegend(0, 2, "Fz");
80 plotter->initGraph(1, 3);
81 plotter->setTitle(1, "Torque measurements");
82 plotter->setLegend(1, 0, "Tx");
83 plotter->setLegend(1, 1, "Ty");
84 plotter->setLegend(1, 2, "Tz");
85 }
86#endif
87
88 if (!iit_ft.connected()) {
89 std::cout << "Unable to connect to IIT force-torque sensor" << std::endl;
90 return EXIT_SUCCESS;
91 }
92
93 iit_ft.bias();
94 iit_ft.startStreaming();
95
96 vpColVector ft;
97 bool end = false;
98 unsigned long nbacq = 0;
99 double t_start = vpTime::measureTimeMs();
100 while (!end) {
101 double t = vpTime::measureTimeMs();
102
103 ft = iit_ft.getForceTorque(opt_filtered);
104#if defined(VISP_HAVE_DISPLAY)
105 if (!opt_no_display) {
106 vpColVector force = ft.extract(0, 3);
107 vpColVector torque = ft.extract(3, 3);
108 plotter->plot(0, nbacq, force);
109 plotter->plot(1, nbacq, torque);
110 vpDisplay::displayText(plotter->I, 20, 80, "Left click to quit", vpColor::red);
112
113 if (vpDisplay::getClick(plotter->I, button, false)) {
114 if (button == vpMouseButton::button1) {
115 end = true;
116 }
117 }
118 vpDisplay::flush(plotter->I);
119 } else {
120 std::cout << "F/T: " << ft.t() << std::endl;
121 if (nbacq > 30) {
122 end = true;
123 }
124 }
125#else
126 std::cout << "F/T: " << ft.t() << std::endl;
127 if (nbacq > 30) {
128 end = true;
129 }
130#endif
131 vpTime::wait(t, 1);
132 nbacq++;
133 }
134
135 iit_ft.stopStreaming();
136 double fps = 1000. * nbacq / (vpTime::measureTimeMs() - t_start);
137 std::cout << "Mean acquisition frequency: " << fps << " Hz" << std::endl;
138
139#if defined(VISP_HAVE_DISPLAY)
140 if (plotter) {
141 delete plotter;
142 }
143#endif
144
145 std::cout << "Test succeed" << std::endl;
146
147#else
148 (void)argc;
149 (void)argv;
150 std::cout << "ViSP is not build with IIT force-torque SDK support" << std::endl;
151#endif
152 return EXIT_SUCCESS;
153}
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)
bool connected(int timeout_ms=0) const
vpColVector getForceTorque(bool filtered=false)
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
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()