45 #include <visp3/core/vpIoTools.h>
46 #include <visp3/core/vpTime.h>
47 #include <visp3/gui/vpPlot.h>
48 #include <visp3/sensor/vpForceTorqueAtiSensor.h>
50 #if defined(VISP_HAVE_THREADS)
55 typedef enum { BIAS_DONE, UNBIAS_DONE, TO_BIAS, TO_UNBIAS } BiasState;
57 #ifdef ENABLE_VISP_NAMESPACE
61 #ifndef DOXYGEN_SHOULD_SKIP_THIS
70 void scopeFunction(std::mutex &mutex_data, std::mutex &mutex_state, t_shared_data &s_shared_data,
bool &s_state_stop)
72 #ifdef VISP_HAVE_DISPLAY
73 vpPlot scope(2, 700, 700, 100, 200,
"ATI F/T sensor data");
74 scope.initGraph(0, 3);
75 scope.initGraph(1, 3);
76 scope.setTitle(0,
"Forces (N)");
77 scope.setTitle(1,
"Torques (Nm)");
78 scope.setLegend(0, 0,
"x");
79 scope.setLegend(0, 1,
"y");
80 scope.setLegend(0, 2,
"z");
81 scope.setLegend(1, 0,
"x");
82 scope.setLegend(1, 1,
"y");
83 scope.setLegend(1, 2,
"z");
89 t_shared_data shared_data;
90 #ifdef VISP_HAVE_DISPLAY
97 #ifdef VISP_HAVE_DISPLAY
99 std::lock_guard<std::mutex> lock(mutex_data);
100 shared_data.ft = s_shared_data.ft;
101 shared_data.timestamp = s_shared_data.timestamp;
102 shared_data.bias_state = s_shared_data.bias_state;
107 scope.plot(0, shared_data.timestamp, force);
108 scope.plot(1, shared_data.timestamp, torque);
112 if (shared_data.bias_state == BIAS_DONE)
114 else if (shared_data.bias_state == UNBIAS_DONE)
116 else if (shared_data.bias_state == TO_BIAS)
118 else if (shared_data.bias_state == TO_UNBIAS)
123 if (shared_data.bias_state == BIAS_DONE)
124 shared_data.bias_state = TO_UNBIAS;
125 else if (shared_data.bias_state == UNBIAS_DONE)
126 shared_data.bias_state = TO_BIAS;
128 std::lock_guard<std::mutex> lock(mutex_data);
129 s_shared_data.bias_state = shared_data.bias_state;
135 #ifdef VISP_HAVE_DISPLAY
142 std::lock_guard<std::mutex> lock(mutex_state);
146 std::cout <<
"End of scope thread" << std::endl;
149 int main(
int argc,
char **argv)
151 #if defined(VISP_HAVE_ATIDAQ) && defined(VISP_HAVE_COMEDI)
153 #ifdef VISP_HAVE_VIPER850_DATA
156 std::string calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string(
"/ati/FT17824.cal");
158 std::cout <<
"ATI F/T calib file \"" << calibfile <<
"\" doesn't exist";
163 std::cout <<
"Usage: " << argv[0] <<
" <ATI calibration file FT*.cal]>" << std::endl;
166 std::string calibfile(argv[1]);
172 std::cout <<
"ATI F/T sensor characteristics: \n" << ati << std::endl;
175 std::cout <<
"Data recording in progress..." << std::endl;
177 std::mutex mutex_data;
178 std::mutex mutex_state;
179 bool state_stop =
false;
180 bool s_state_stop =
false;
181 t_shared_data shared_data;
182 t_shared_data s_shared_data;
185 std::thread thread_scope(&scopeFunction, std::ref(mutex_data), std::ref(mutex_state), std::ref(s_shared_data), std::ref(s_state_stop));
187 std::string file(
"recorded-ft-sync.txt");
188 std::ofstream f(file.c_str());
195 double timestamp = loop_time - start_time;
198 std::lock_guard<std::mutex> lock(mutex_data);
199 shared_data.bias_state = s_shared_data.bias_state;
201 if (shared_data.bias_state == TO_BIAS) {
202 std::cout <<
"Bias sensor" << std::endl;
204 std::cout <<
"Unbias sensor" << std::endl;
205 shared_data.bias_state = BIAS_DONE;
207 else if (shared_data.bias_state == TO_UNBIAS) {
209 shared_data.bias_state = UNBIAS_DONE;
213 std::lock_guard<std::mutex> lock(mutex_data);
214 s_shared_data.ft = ft;
215 s_shared_data.timestamp = timestamp;
216 s_shared_data.bias_state = shared_data.bias_state;
219 std::lock_guard<std::mutex> lock(mutex_state);
220 state_stop = s_state_stop;
223 f << timestamp <<
" " << ft.
t() << std::endl;
225 }
while (!state_stop);
232 std::cout <<
"Data recorded in " << file << std::endl;
236 std::cout <<
"You should install comedi and build atidaq to enable this test..." << std::endl;
244 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)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()