Visual Servoing Platform  version 3.4.1 under development (2021-12-05)
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();
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  }
211  else {
212  return m_ft;
213  }
214 }
215 
216 
221 {
222  m_acquisitionEnabled = true;
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
244 void dummy_vpForceTorqueIitSensor(){};
245 #endif
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:173
double getDurationMs()
Definition: vpTime.cpp:392
std::atomic< bool > m_acquisitionEnabled
bool connected(int timeout_ms=0) const
void start(bool reset=true)
Definition: vpTime.cpp:409
VISP_EXPORT void sleepMs(double t)
Definition: vpTime.cpp:271
std::atomic< bool > m_dataValid
std::chrono::time_point< std::chrono::system_clock > m_timePrev
std::chrono::time_point< std::chrono::system_clock > m_timeCur
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpColVector getForceTorque(bool filtered=false)
ftSensorsConnected m_ftSensorsData