Visual Servoing Platform  version 3.6.1 under development (2024-10-14)
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 
48 BEGIN_VISP_NAMESPACE
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  }
145  else {
146  continue;
147  }
148 
149  const std::lock_guard<std::mutex> lock(m_mutex);
150  for (unsigned int i = 0; i < 6; i++) {
151  m_ft[i] = m_ftSensorsData.ftSensor->ft[i];
152  m_ft_filt[i] = m_ftSensorsData.ftSensor->filt_ft[i];
153  }
154  }
155  }
156 }
157 
161 void vpForceTorqueIitSensor::bias() { m_ftLib._biasAllFTSensorsTCP(); }
162 
174 bool vpForceTorqueIitSensor::connected(int timeout_ms) const
175 {
176  vpChrono chrono;
177  chrono.start();
178  while (!m_connected && chrono.getDurationMs() < timeout_ms) {
179  vpTime::sleepMs(1);
180  }
181 
182  return m_connected;
183 }
184 
208 {
209  const std::lock_guard<std::mutex> lock(m_mutex);
210  if (filtered) {
211  return m_ft_filt;
212  }
213  else {
214  return m_ft;
215  }
216 }
217 
222 {
223  m_acquisitionEnabled = true;
224  m_acquisitionThread = std::thread([this] { this->acquisitionLoop(); });
225 
226  while (!m_dataValid) {
227  vpTime::wait(10);
228  }
229 }
230 
235 {
236  m_acquisitionEnabled = false;
237  if (m_acquisitionThread.joinable()) {
238  m_acquisitionThread.join();
239  }
240 }
241 END_VISP_NAMESPACE
242 #else
243 // Work around to avoid warning:
244 // libvisp_sensor.a(vpForceTorqueIitSensor.cpp.o) has no symbols
245 void dummy_vpForceTorqueIitSensor() { };
246 #endif
void start(bool reset=true)
Definition: vpTime.cpp:401
double getDurationMs()
Definition: vpTime.cpp:390
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
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)