Visual Servoing Platform  version 3.6.1 under development (2024-03-29)
vpForceTorqueIitSensor.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  * Wrapper over IIT force-torque sensor.
33  *
34  * Authors:
35  * Alexander Oliva
36  *
37 *****************************************************************************/
38 
44 #include <visp3/sensor/vpForceTorqueIitSensor.h>
45 
46 #if defined(VISP_HAVE_FT_IIT_SDK) && defined(VISP_HAVE_THREADS)
47 
54  : m_ftLib(), m_numSensorsInLib(0), m_ft(6, 0), m_ft_filt(6, 0), m_ftSensorsData(), m_acquisitionEnabled(false),
55  m_dataValid(false), m_connected(false), m_acquisitionThread(), m_timeCur(), m_timePrev(), m_mutex(),
56  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();
86  m_acquisitionEnabled = false;
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 
159 void vpForceTorqueIitSensor::bias() { m_ftLib._biasAllFTSensorsTCP(); }
160 
172 bool vpForceTorqueIitSensor::connected(int timeout_ms) const
173 {
174  vpChrono chrono;
175  chrono.start();
176  while (!m_connected && chrono.getDurationMs() < timeout_ms) {
177  vpTime::sleepMs(1);
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  } else {
211  return m_ft;
212  }
213 }
214 
219 {
220  m_acquisitionEnabled = true;
221  m_acquisitionThread = std::thread([this] { this->acquisitionLoop(); });
222 
223  while (!m_dataValid) {
224  vpTime::wait(10);
225  }
226 }
227 
232 {
233  m_acquisitionEnabled = false;
234  if (m_acquisitionThread.joinable()) {
235  m_acquisitionThread.join();
236  }
237 }
238 
239 #else
240 // Work around to avoid warning:
241 // libvisp_sensor.a(vpForceTorqueIitSensor.cpp.o) has no symbols
242 void dummy_vpForceTorqueIitSensor(){};
243 #endif
void start(bool reset=true)
Definition: vpTime.cpp:397
double getDurationMs()
Definition: vpTime.cpp:386
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
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)