Visual Servoing Platform version 3.5.0
vpForceTorqueIitSensor.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 * Wrapper over IIT force-torque sensor.
33 *
34 * Authors:
35 * Fabien Spindler
36 * Alexander Oliva
37 *
38 *****************************************************************************/
39
45#include <visp3/sensor/vpForceTorqueIitSensor.h>
46
47#ifdef VISP_HAVE_FT_IIT_SDK
48
55 : m_ftLib(), m_numSensorsInLib(0), m_ft(6, 0), m_ft_filt(6, 0), m_ftSensorsData(), m_acquisitionEnabled(false), m_dataValid(false),
56 m_connected(false), m_acquisitionThread(), m_timeCur(), m_timePrev(), m_mutex(), m_warmupMilliseconds(500)
57{
58 // Get number of connected in library sensors
59 m_numSensorsInLib = m_ftLib._getNumberOfConnectedSensors();
60
61 /*
62 * Initialize Communication with sensor(-s):
63 *
64 * streaming is configured with "storeOption=0",
65 * which means that data will be stored in the library's main thread
66 * and will be given in an output file.
67 *
68 * "storeDataFlag" is initialized as "false", so that the
69 * recording will not start with the streaming thread, but
70 * the moment for when the recording will start & stop.
71 */
72 if (m_ftLib._configureStreaming(false, 0) == 0) {
73 // Start the main acquisition thread
74 m_ftLib._startStreamingThread();
75
76 m_connected = true;
77 }
78}
79
84{
85 m_ftLib._stopStreamingThread();
87 m_dataValid = false;
88}
89
94
99{
100 if (m_acquisitionThread.joinable()) {
101 m_acquisitionThread.join();
102 }
103}
104
109{
110 m_timePrev = m_timeCur = std::chrono::system_clock::now();
111
112 // Main thread
113 auto time_init = std::chrono::system_clock::now();
114 while (m_acquisitionEnabled) {
115
116 // Get time passed since the last acquired sample
117 m_timeCur = std::chrono::system_clock::now();
118
119 // Calculate delta time
120 auto elapsed_milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(m_timeCur - m_timePrev).count();
121
122 if (elapsed_milliseconds >= 1) {
123 /*
124 * Once a new aquisition started,
125 * reset loop timers to keep a relatively fixed sampling time
126 */
127 // Update previous time
129
130 /*
131 * get all connected sensors' data:
132 * call to a mutex method allowing to access the ftsensors' data
133 * in streaming mode (1 sample / packet / sensor)
134 */
135 m_ftSensorsData = m_ftLib._getFTSensorsData();
136
137 // Warm up the sensor. At the beginning we experienced that values returned in m_ftSensorsData.ftSensor->ft
138 // are completly wrong like the following:
139 // 2.237378396e+11 207.3293304 14291.07715 1.479413346e+19 13.26593399 3380.078613
140 auto warmup_milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(m_timeCur - time_init).count();
141 if (warmup_milliseconds > m_warmupMilliseconds) {
142 m_dataValid = true;
143 } else {
144 continue;
145 }
146
147 const std::lock_guard<std::mutex> lock(m_mutex);
148 for (unsigned int i = 0; i < 6; i++) {
149 m_ft[i] = m_ftSensorsData.ftSensor->ft[i];
150 m_ft_filt[i] = m_ftSensorsData.ftSensor->filt_ft[i];
151 }
152 }
153 }
154}
155
159void vpForceTorqueIitSensor::bias() { m_ftLib._biasAllFTSensorsTCP(); }
160
172bool vpForceTorqueIitSensor::connected(int timeout_ms) const
173{
174 vpChrono chrono;
175 chrono.start();
176 while (!m_connected && chrono.getDurationMs() < timeout_ms) {
178 }
179
180 return m_connected;
181}
182
206{
207 const std::lock_guard<std::mutex> lock(m_mutex);
208 if (filtered) {
209 return m_ft_filt;
210 }
211 else {
212 return m_ft;
213 }
214}
215
216
221{
223 m_acquisitionThread = std::thread([this] { this->acquisitionLoop(); });
224
225 while (!m_dataValid) {
226 vpTime::wait(10);
227 }
228}
229
234{
235 m_acquisitionEnabled = false;
236 if (m_acquisitionThread.joinable()) {
237 m_acquisitionThread.join();
238 }
239}
240
241#else
242// Work arround to avoid warning:
243// libvisp_sensor.a(vpForceTorqueIitSensor.cpp.o) has no symbols
244void dummy_vpForceTorqueIitSensor(){};
245#endif
void start(bool reset=true)
Definition: vpTime.cpp:409
double getDurationMs()
Definition: vpTime.cpp:392
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
std::chrono::time_point< std::chrono::system_clock > m_timePrev
std::atomic< bool > m_dataValid
bool connected(int timeout_ms=0) const
std::chrono::time_point< std::chrono::system_clock > m_timeCur
ftSensorsConnected m_ftSensorsData
vpColVector getForceTorque(bool filtered=false)
std::atomic< bool > m_acquisitionEnabled
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT void sleepMs(double t)