ViSP
SickLDMRS-Process.cpp
1 /****************************************************************************
2  *
3  * $Id: SickLDMRS-Process.cpp 4658 2014-02-09 09:50:14Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Sick LD-MRS laser driver.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
42 
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>
76 
77 #if ( !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) ) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
78 
79 static int save = 0;
80 static int layerToDisplay = 0xF; // 0xF = 1111 => all the layers are selected
81 static vpLaserScan shm_laserscan[4];
82 double time_offset = 0;
83 #ifdef VISP_HAVE_PTHREAD
84 pthread_mutex_t shm_mutex;
85 #endif
86 std::string output_path;
87 
88 void *laser_display_and_save_loop(void *);
89 void *laser_acq_loop(void *);
90 void *camera_acq_and_display_loop(void *);
91 
92 void *laser_display_and_save_loop(void *)
93 {
94  vpImage<unsigned char> map(700, 300);
95  map = 0;
96  unsigned int width = map.getWidth();
97  unsigned int height = map.getHeight();
98  vpImagePoint O; // Beam origin
99  O.set_i(height);
100  O.set_j(width/2.);
101  vpScanPoint p;
102  vpColor color[4]; // one color per layer
103  char filename[FILENAME_MAX];
104  std::ofstream fdscan;
105  vpLaserScan laserscan[4];
106 
107  for (int layer=0; layer<4; layer++) {
108  switch (layer) {
109  case 0: color[layer] = vpColor::red; break;
110  case 1: color[layer] = vpColor::green; break;
111  case 2: color[layer] = vpColor::blue; break;
112  case 3: color[layer] = vpColor::yellow; break;
113  }
114  }
115 
116  vpDisplay *display;
117 #if defined VISP_HAVE_X11
118  display = new vpDisplayX;
119 #elif defined VISP_HAVE_GDI
120  display = new vpDisplayGDI;
121 #elif defined VISP_HAVE_GTK
122  display = new vpDisplayGTK;
123 #endif
124  display->init (map, 10, 10, "Laser scan");
125 
126  unsigned int iter = 0;
127  for ( ; ; ) {
128 
129 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
130  vpDisplay::display(map);
131 #endif
132 
133 #ifdef VISP_HAVE_PTHREAD
134  pthread_mutex_lock(&shm_mutex);
135 #endif
136  for (int layer=0; layer < 4; layer++)
137  laserscan[layer] = shm_laserscan[layer];
138 #ifdef VISP_HAVE_PTHREAD
139  pthread_mutex_unlock(&shm_mutex);
140 #endif
141 
142  // std::cout << "laser start timestamp "
143  // << laserscan[0].getStartTimestamp() - time_offset << std::endl;
144 
145  // Parse the four layers
146  for (int layer=0; layer<4; layer++) {
147  if (! ((0x1<<layer) & layerToDisplay)) {
148  std::cout << "Layer " << layer+1 << " is not displayed" << std::endl;
149  continue;
150  }
151 
152  std::vector<vpScanPoint> pointsLayer = laserscan[layer].getScanPoints();
153 
154  if (save) {
155  // Set the scan data filename to store the measures
156  sprintf(filename, "%s/scan%04d-layer%d.txt",
157  output_path.c_str(), iter, layer+1);
158  fdscan.open(filename);
159 
160  // Write the file header
161  fdscan << "# Scan layer [1 to 4] : " << layer+1 << std::endl
162  << "# Start timestamp (s) : "
163  << laserscan[layer].getStartTimestamp() - time_offset
164  << std::endl
165  << "# End timestamp (s) : "
166  << laserscan[layer].getEndTimestamp() - time_offset
167  << std::endl
168  << "# Data : \"radial distance (m)\" \"horizontal angle (rad)\" \"vertical angle (rad)\" \"X (m)\" \"Y (m)\" \"Z (m)\""
169  << std::endl;
170  }
171 
172 
173  vpImagePoint E; // Beam echo
174  double resolution = 5; // 100 pixels = 1 meter - increase this value to see better near info
175  // std::cout << "display layer " << layer << " nb points: "
176  // << pointsLayer.size() << std::endl;
177  for (unsigned int i=0; i<pointsLayer.size(); i++) {
178  p = pointsLayer[i];
179  E.set_i(height - resolution * p.getRadialDist() * cos(p.getHAngle()));
180  E.set_j(width/2. - resolution * p.getRadialDist() * sin(p.getHAngle()));
181  //std::cout << "E: " << E << std::endl;
182 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
183  vpDisplay::displayLine(map, O, E, color[layer]);
184 #endif
185  if (save) {
186  // Save the measures in the file
187  fdscan << p << std::endl;
188  }
189  }
190  if (save) {
191  fdscan.close();
192  }
193  }
194 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
195  vpDisplay::flush(map);
196 #endif
197  iter ++;
198  //std::cout << "display time: " << vpTime::measureTimeMs() - t1 << std::endl;
199 
200  }
201  delete display;
202  return NULL;
203 }
204 
205 void *laser_acq_loop(void *)
206 {
207  std::string ip = "131.254.12.119";
208 
209  vpSickLDMRS laser;
210  laser.setIpAddress(ip);
211  laser.setup();
212  vpLaserScan laserscan[4];
213 
214  unsigned int iter = 0;
215  for ( ; ; ) {
216  double t1 = vpTime::measureTimeMs();
217  if (laser.measure(laserscan) == false)
218  continue;
219 
220 #ifdef VISP_HAVE_PTHREAD
221  pthread_mutex_lock(&shm_mutex);
222 #endif
223  for (int layer=0; layer < 4; layer++)
224  shm_laserscan[layer] = laserscan[layer];
225 #ifdef VISP_HAVE_PTHREAD
226  pthread_mutex_unlock(&shm_mutex);
227 #endif
228 
229  iter ++;
230  std::cout << "laser acq time: " << vpTime::measureTimeMs() - t1 << std::endl;
231 
232  }
233 
234  return NULL;
235 }
236 
237 void *camera_acq_and_display_loop(void *)
238 {
239 #ifdef VISP_HAVE_DC1394_2
240  try {
241  // Initialize the firewire framegrabber
242  vp1394TwoGrabber g; // Create a grabber based on libdc1394-2.x third party lib
243 
244  // If no camera found return
245  if (g.getNumCameras() == 0)
246  return NULL;
247 
248  // g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
249  // g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
250 
251  vpImage<unsigned char> I; // Create a gray level image container
252  vpImage<unsigned char> Q; // Create a quarter size gray level image container
253  g.acquire(I); // Acquire an image
254  I.quarterSizeImage(Q);
255 
256  vpDisplay *display;
257 #if defined VISP_HAVE_X11
258  display = new vpDisplayX;
259 #elif defined VISP_HAVE_GDI
260  display = new vpDisplayGDI;
261 #elif defined VISP_HAVE_GTK
262  display = new vpDisplayGTK;
263 #endif
264  display->init (Q, 320, 10, "Camera");
265 
266  // Create a file with cameraimage time stamps
267  std::ofstream fdimage_ts;
268  if (save) {
269  std::string filename = output_path + "/image_timestamp.txt";
270  fdimage_ts.open( filename.c_str() );
271  fdimage_ts << "# [image name] [time stamp in second]" << std::endl;
272  }
273  dc1394video_frame_t *frame;
274  unsigned iter = 0;
275  char filename[FILENAME_MAX];
276  uint64_t timestamp;
277  uint32_t id;
278  double image_timestamp;
279  for ( ; ; ) {
280  frame = g.dequeue(I, timestamp, id); // Acquire an image
281  I.quarterSizeImage(Q);
282  image_timestamp = timestamp/1000000. - time_offset;
283  std::cout << "camera timestamp: " << image_timestamp << " s " << std::endl;
284  if (save) {
285  // Set the image filename
286  sprintf(filename, "%s/image%04d.png", output_path.c_str(), iter);
287  vpImageIo::write(Q, filename);
288  fdimage_ts << filename << " " << image_timestamp << std::endl;
289  }
290 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
292  vpDisplay::flush(Q);
293 #endif
294  g.enqueue(frame);
295 
296  iter ++;
297  }
298  delete display;
299  if (save) {
300  fdimage_ts.close();
301  }
302  }
303  catch(...) {
304 
305  }
306 #endif
307  return NULL;
308 }
309 
310 
311 int main(int argc, const char ** argv)
312 {
313  try {
314  output_path = "data";
315  // Test if the output path directory exist. If no try to create it
316  if (vpIoTools::checkDirectory(output_path) == false) {
317  try {
318  // Create a directory with name "username"
319  vpIoTools::makeDirectory(output_path);
320  }
321  catch (...) {
322  std::cout << "Cannot create " << output_path << " directory" << std::endl;
323  return false;
324  }
325  }
326 
327  // Parse the command line to set the variables
328  vpParseArgv::vpArgvInfo argTable[] =
329  {
330  {"-layer", vpParseArgv::ARGV_INT, (char*) NULL, (char *) &layerToDisplay,
331  "The layer to display:\n"
332  "\t\t. 0x1 for layer 1.\n"
333  "\t\t. 0x2 for layer 2.\n"
334  "\t\t. 0x4 for layer 3.\n"
335  "\t\t. 0x8 for layer 4.\n"
336  "\t\tTo display all the layers you should set 0xF value."
337  },
338  {"-save", vpParseArgv::ARGV_INT, (char*) NULL, (char *) &save,
339  "Turn to 1 in order to save data."
340  },
341  {"-h", vpParseArgv::ARGV_HELP, (char*) NULL, (char *) NULL,
342  "Display one or more measured layers form a Sick LD-MRS laser scanner."},
343  {(char*) NULL, vpParseArgv::ARGV_END, (char*) NULL, (char*) NULL, (char*) NULL}
344  } ;
345 
346  // Read the command line options
347  if(vpParseArgv::parse(&argc, argv, argTable,
351  return (false);
352  }
353 
354  time_offset = vpTime::measureTimeSecond();
355 #ifdef VISP_HAVE_PTHREAD
356  pthread_t thread_camera_acq;
357  pthread_t thread_laser_acq;
358  pthread_t thread_laser_display;
359  pthread_create(&thread_camera_acq, NULL, &camera_acq_and_display_loop, NULL);
360  pthread_create(&thread_laser_acq, NULL, &laser_acq_loop, NULL);
361  pthread_create(&thread_laser_display, NULL, &laser_display_and_save_loop, NULL);
362  pthread_join(thread_camera_acq, 0);
363  pthread_join(thread_laser_acq, 0);
364  pthread_join(thread_laser_display, 0);
365 #endif
366 
367  return 0;
368  }
369  catch(vpException e) {
370  std::cout << "Catch an exception: " << e << std::endl;
371  return 1;
372  }
373 }
374 
375 #else // #ifdef UNIX and display
376 
377 int main()
378 {
379  std::cout << "This example is only working on UNIX platforms \n"
380  << "since the Sick LD-MRS driver was not ported to Windows."
381  << std::endl;
382  return 0;
383 }
384 
385 #endif // #ifdef UNIX
static void write(const vpImage< unsigned char > &I, const char *filename)
Definition: vpImageIo.cpp:476
virtual void init(vpImage< unsigned char > &I, int x=-1, int y=-1, const char *title=NULL)=0
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:174
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:315
static void close(vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2269
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
Define the X11 console to display images.
Definition: vpDisplayX.h:152
error that can be emited by ViSP classes.
Definition: vpException.h:76
std::vector< vpScanPoint > getScanPoints()
Definition: vpLaserScan.h:100
void getNumCameras(unsigned int &ncameras) const
static double measureTimeMs()
Definition: vpTime.cpp:86
static const vpColor green
Definition: vpColor.h:170
void acquire(vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2232
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:80
static const vpColor red
Definition: vpColor.h:167
Implements a laser scan data structure that contains especially the list of scanned points that have ...
Definition: vpLaserScan.h:66
static void makeDirectory(const char *dirname)
Definition: vpIoTools.cpp:384
void quarterSizeImage(vpImage< Type > &res)
Definition: vpImage.h:972
void set_i(const double ii)
Definition: vpImagePoint.h:159
Class that defines a single laser scanner point.
Definition: vpScanPoint.h:75
double getRadialDist() const
Definition: vpScanPoint.h:115
double getHAngle() const
Definition: vpScanPoint.h:127
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:210
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:145
void enqueue(dc1394video_frame_t *frame)
void set_j(const double jj)
Definition: vpImagePoint.h:170
Command line argument parsing.
Definition: vpParseArgv.h:134
double getStartTimestamp()
Definition: vpLaserScan.h:133
dc1394video_frame_t * dequeue()
Class for firewire ieee1394 video devices using libdc1394-2.x api.
static double measureTimeSecond()
Definition: vpTime.cpp:225
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:93
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
static const vpColor yellow
Definition: vpColor.h:175
static const vpColor blue
Definition: vpColor.h:173
double getEndTimestamp()
Definition: vpLaserScan.h:137