47 #include <visp3/core/vpIoTools.h>
48 #include <visp3/core/vpTime.h>
49 #include <visp3/gui/vpPlot.h>
50 #include <visp3/sensor/vpForceTorqueAtiSensor.h>
52 #if defined(VISP_HAVE_THREADS)
57 typedef enum { BIAS_DONE, UNBIAS_DONE, TO_BIAS, TO_UNBIAS } BiasState;
59 #ifndef DOXYGEN_SHOULD_SKIP_THIS
68 void scopeFunction(std::mutex &mutex_data, std::mutex &mutex_state, t_shared_data &s_shared_data,
bool &s_state_stop)
70 #ifdef VISP_HAVE_DISPLAY
71 vpPlot scope(2, 700, 700, 100, 200,
"ATI F/T sensor data");
72 scope.initGraph(0, 3);
73 scope.initGraph(1, 3);
74 scope.setTitle(0,
"Forces (N)");
75 scope.setTitle(1,
"Torques (Nm)");
76 scope.setLegend(0, 0,
"x");
77 scope.setLegend(0, 1,
"y");
78 scope.setLegend(0, 2,
"z");
79 scope.setLegend(1, 0,
"x");
80 scope.setLegend(1, 1,
"y");
81 scope.setLegend(1, 2,
"z");
87 t_shared_data shared_data;
88 #ifdef VISP_HAVE_DISPLAY
95 #ifdef VISP_HAVE_DISPLAY
97 std::lock_guard<std::mutex> lock(mutex_data);
98 shared_data.ft = s_shared_data.ft;
99 shared_data.timestamp = s_shared_data.timestamp;
100 shared_data.bias_state = s_shared_data.bias_state;
105 scope.plot(0, shared_data.timestamp, force);
106 scope.plot(1, shared_data.timestamp, torque);
110 if (shared_data.bias_state == BIAS_DONE)
112 else if (shared_data.bias_state == UNBIAS_DONE)
114 else if (shared_data.bias_state == TO_BIAS)
116 else if (shared_data.bias_state == TO_UNBIAS)
121 if (shared_data.bias_state == BIAS_DONE)
122 shared_data.bias_state = TO_UNBIAS;
123 else if (shared_data.bias_state == UNBIAS_DONE)
124 shared_data.bias_state = TO_BIAS;
126 std::lock_guard<std::mutex> lock(mutex_data);
127 s_shared_data.bias_state = shared_data.bias_state;
133 #ifdef VISP_HAVE_DISPLAY
140 std::lock_guard<std::mutex> lock(mutex_state);
144 std::cout <<
"End of scope thread" << std::endl;
147 int main(
int argc,
char **argv)
149 #if defined(VISP_HAVE_ATIDAQ) && defined(VISP_HAVE_COMEDI)
151 #ifdef VISP_HAVE_VIPER850_DATA
154 std::string calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string(
"/ati/FT17824.cal");
156 std::cout <<
"ATI F/T calib file \"" << calibfile <<
"\" doesn't exist";
161 std::cout <<
"Usage: " << argv[0] <<
" <ATI calibration file FT*.cal]>" << std::endl;
164 std::string calibfile(argv[1]);
170 std::cout <<
"ATI F/T sensor characteristics: \n" << ati << std::endl;
173 std::cout <<
"Data recording in progress..." << std::endl;
175 std::mutex mutex_data;
176 std::mutex mutex_state;
177 bool state_stop =
false;
178 bool s_state_stop =
false;
179 t_shared_data shared_data;
180 t_shared_data s_shared_data;
183 std::thread thread_scope(&scopeFunction, std::ref(mutex_data), std::ref(mutex_state), std::ref(s_shared_data), std::ref(s_state_stop));
185 std::string file(
"recorded-ft-sync.txt");
186 std::ofstream f(file.c_str());
193 double timestamp = loop_time - start_time;
196 std::lock_guard<std::mutex> lock(mutex_data);
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;
205 else if (shared_data.bias_state == TO_UNBIAS) {
207 shared_data.bias_state = UNBIAS_DONE;
211 std::lock_guard<std::mutex> lock(mutex_data);
212 s_shared_data.ft = ft;
213 s_shared_data.timestamp = timestamp;
214 s_shared_data.bias_state = shared_data.bias_state;
217 std::lock_guard<std::mutex> lock(mutex_state);
218 state_stop = s_state_stop;
221 f << timestamp <<
" " << ft.
t() << std::endl;
223 }
while (!state_stop);
230 std::cout <<
"Data recorded in " << file << std::endl;
234 std::cout <<
"You should install comedi and build atidaq to enable this test..." << std::endl;
242 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()