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