47#include <visp3/core/vpIoTools.h>
48#include <visp3/core/vpMutex.h>
49#include <visp3/core/vpThread.h>
50#include <visp3/core/vpTime.h>
51#include <visp3/gui/vpPlot.h>
52#include <visp3/sensor/vpForceTorqueAtiSensor.h>
54#if defined(VISP_HAVE_PTHREAD)
56typedef enum { BIAS_DONE, UNBIAS_DONE, TO_BIAS, TO_UNBIAS } BiasState;
60#ifndef DOXYGEN_SHOULD_SKIP_THIS
68t_shared_data s_shared_data;
71bool s_state_stop =
false;
77#ifdef VISP_HAVE_DISPLAY
78 vpPlot scope(2, 700, 700, 100, 200,
"ATI F/T sensor data");
79 scope.initGraph(0, 3);
80 scope.initGraph(1, 3);
81 scope.setTitle(0,
"Forces (N)");
82 scope.setTitle(1,
"Torques (Nm)");
83 scope.setLegend(0, 0,
"x");
84 scope.setLegend(0, 1,
"y");
85 scope.setLegend(0, 2,
"z");
86 scope.setLegend(1, 0,
"x");
87 scope.setLegend(1, 1,
"y");
88 scope.setLegend(1, 2,
"z");
91 t_shared_data shared_data;
92#ifdef VISP_HAVE_DISPLAY
99#ifdef VISP_HAVE_DISPLAY
102 shared_data.ft = s_shared_data.ft;
103 shared_data.timestamp = s_shared_data.timestamp;
104 shared_data.bias_state = s_shared_data.bias_state;
109 scope.plot(0, shared_data.timestamp, force);
110 scope.plot(1, shared_data.timestamp, torque);
114 if (shared_data.bias_state == BIAS_DONE)
116 else if (shared_data.bias_state == UNBIAS_DONE)
118 else if (shared_data.bias_state == TO_BIAS)
120 else if (shared_data.bias_state == TO_UNBIAS)
125 if (shared_data.bias_state == BIAS_DONE)
126 shared_data.bias_state = TO_UNBIAS;
127 else if (shared_data.bias_state == UNBIAS_DONE)
128 shared_data.bias_state = TO_BIAS;
131 s_shared_data.bias_state = shared_data.bias_state;
137#ifdef VISP_HAVE_DISPLAY
148 std::cout <<
"End of scope thread" << std::endl;
152int main(
int argc,
char **argv)
154#if defined(VISP_HAVE_ATIDAQ) && defined(VISP_HAVE_COMEDI)
156#ifdef VISP_HAVE_VIPER850_DATA
159 std::string calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string(
"/ati/FT17824.cal");
161 std::cout <<
"ATI F/T calib file \"" << calibfile <<
"\" doesn't exist";
166 std::cout <<
"Usage: " << argv[0] <<
" <ATI calibration file FT*.cal]>" << std::endl;
169 std::string calibfile(argv[1]);
175 std::cout <<
"ATI F/T sensor characteristics: \n" << ati << std::endl;
178 std::cout <<
"Data recording in progress..." << std::endl;
181 vpThread thread_scope(scopeFunction);
183 std::string file(
"recorded-ft-sync.txt");
184 std::ofstream f(file.c_str());
186 t_shared_data shared_data;
193 double timestamp = loop_time - start_time;
197 shared_data.bias_state = s_shared_data.bias_state;
199 if (shared_data.bias_state == TO_BIAS) {
200 std::cout <<
"Bias sensor" << std::endl;
202 std::cout <<
"Unbias sensor" << std::endl;
203 shared_data.bias_state = BIAS_DONE;
204 }
else if (shared_data.bias_state == TO_UNBIAS) {
206 shared_data.bias_state = UNBIAS_DONE;
211 s_shared_data.ft = ft;
212 s_shared_data.timestamp = timestamp;
213 s_shared_data.bias_state = shared_data.bias_state;
217 state_stop = s_state_stop;
220 f << timestamp <<
" " << ft.
t() << std::endl;
222 }
while (!state_stop);
229 std::cout <<
"Data recorded in " << file << std::endl;
233 std::cout <<
"You should install comedi and build atidaq to enable this test..." << std::endl;
241 std::cout <<
"You should build this test with threading capabilities..." << std::endl;
Implementation of column vector and the associated operations.
vpColVector extract(unsigned int r, unsigned int colsize) const
static const vpColor blue
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
vpColVector getForceTorque() const
void setCalibrationFile(const std::string &calibfile, unsigned short index=1)
Class that allows protection by mutex.
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
VISP_EXPORT double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)