64 #include <visp/vpDebug.h>
65 #include <visp/vpImagePoint.h>
66 #include <visp/vpSickLDMRS.h>
67 #include <visp/vpImage.h>
68 #include <visp/vpImageIo.h>
69 #include <visp/vpDisplay.h>
70 #include <visp/vpDisplayX.h>
71 #include <visp/vpDisplayGDI.h>
72 #include <visp/vpDisplayGTK.h>
73 #include <visp/vpParseArgv.h>
74 #include <visp/vp1394TwoGrabber.h>
75 #include <visp/vpIoTools.h>
77 #if ( defined(UNIX) && ( ! defined(WIN32) ) ) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
80 static int layerToDisplay = 0xF;
82 double time_offset = 0;
83 #ifdef VISP_HAVE_PTHREAD
84 pthread_mutex_t shm_mutex;
86 std::string output_path;
89 void *laser_display_and_save_loop(
void *)
93 unsigned int width = map.getWidth();
94 unsigned int height = map.getHeight();
100 char filename[FILENAME_MAX];
101 std::ofstream fdscan;
104 for (
int layer=0; layer<4; layer++) {
114 #if defined VISP_HAVE_X11
116 #elif defined VISP_HAVE_GDI
118 #elif defined VISP_HAVE_GTK
121 display->
init (map, 10, 10,
"Laser scan");
123 unsigned int iter = 0;
126 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
130 #ifdef VISP_HAVE_PTHREAD
131 pthread_mutex_lock(&shm_mutex);
133 for (
int layer=0; layer < 4; layer++)
134 laserscan[layer] = shm_laserscan[layer];
135 #ifdef VISP_HAVE_PTHREAD
136 pthread_mutex_unlock(&shm_mutex);
143 for (
int layer=0; layer<4; layer++) {
144 if (! ((0x1<<layer) & layerToDisplay)) {
145 std::cout <<
"Layer " << layer+1 <<
" is not displayed" << std::endl;
149 std::vector<vpScanPoint> pointsLayer = laserscan[layer].
getScanPoints();
153 sprintf(filename,
"%s/scan%04d-layer%d.txt",
154 output_path.c_str(), iter, layer+1);
155 fdscan.open(filename);
158 fdscan <<
"# Scan layer [1 to 4] : " << layer+1 << std::endl
159 <<
"# Start timestamp (s) : "
162 <<
"# End timestamp (s) : "
165 <<
"# Data : \"radial distance (m)\" \"horizontal angle (rad)\" \"vertical angle (rad)\" \"X (m)\" \"Y (m)\" \"Z (m)\""
171 double resolution = 5;
174 for (
unsigned int i=0; i<pointsLayer.size(); i++) {
179 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
184 fdscan << p << std::endl;
191 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
202 void *laser_acq_loop(
void *)
204 std::string ip =
"131.254.12.119";
207 laser.setIpAddress(ip);
211 unsigned int iter = 0;
214 if (laser.measure(laserscan) ==
false)
217 #ifdef VISP_HAVE_PTHREAD
218 pthread_mutex_lock(&shm_mutex);
220 for (
int layer=0; layer < 4; layer++)
221 shm_laserscan[layer] = laserscan[layer];
222 #ifdef VISP_HAVE_PTHREAD
223 pthread_mutex_unlock(&shm_mutex);
234 void *camera_acq_and_display_loop(
void *)
236 #ifdef VISP_HAVE_DC1394_2
254 #if defined VISP_HAVE_X11
256 #elif defined VISP_HAVE_GDI
258 #elif defined VISP_HAVE_GTK
261 display->
init (Q, 320, 10,
"Camera");
264 std::ofstream fdimage_ts;
266 std::string filename = output_path +
"/image_timestamp.txt";
267 fdimage_ts.open( filename.c_str() );
268 fdimage_ts <<
"# [image name] [time stamp in second]" << std::endl;
270 dc1394video_frame_t *frame;
272 char filename[FILENAME_MAX];
275 double image_timestamp;
277 frame = g.
dequeue(I, timestamp,
id);
279 image_timestamp = timestamp/1000000. - time_offset;
280 std::cout <<
"camera timestamp: " << image_timestamp <<
" s " << std::endl;
283 sprintf(filename,
"%s/image%04d.png", output_path.c_str(), iter);
285 fdimage_ts << filename <<
" " << image_timestamp << std::endl;
287 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
308 int main(
int argc,
const char ** argv)
310 output_path =
"data";
318 std::cout <<
"Cannot create " << output_path <<
" directory" << std::endl;
324 vpParseArgv::vpArgvInfo argTable[] =
327 "The layer to display:\n"
328 "\t\t. 0x1 for layer 1.\n"
329 "\t\t. 0x2 for layer 2.\n"
330 "\t\t. 0x4 for layer 3.\n"
331 "\t\t. 0x8 for layer 4.\n"
332 "\t\tTo display all the layers you should set 0xF value."
335 "Turn to 1 in order to save data."
338 "Display one or more measured layers form a Sick LD-MRS laser scanner."},
351 #ifdef VISP_HAVE_PTHREAD
352 pthread_t thread_camera_acq;
353 pthread_t thread_laser_acq;
354 pthread_t thread_laser_display;
355 pthread_create(&thread_camera_acq, NULL, &camera_acq_and_display_loop, NULL);
356 pthread_create(&thread_laser_acq, NULL, &laser_acq_loop, NULL);
357 pthread_create(&thread_laser_display, NULL, &laser_display_and_save_loop, NULL);
358 pthread_join(thread_camera_acq, 0);
359 pthread_join(thread_laser_acq, 0);
360 pthread_join(thread_laser_display, 0);
366 #else // #ifdef UNIX and display
370 std::cout <<
"This example is only working on UNIX platforms \n"
371 <<
"since the Sick LD-MRS driver was not ported to Windows."
376 #endif // #ifdef UNIX