ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
SickLDMRS-Process.cpp
1 /****************************************************************************
2  *
3  * $Id: SickLDMRS-Process.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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(UNIX) && ( ! defined(WIN32) ) ) && (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 
89 void *laser_display_and_save_loop(void *)
90 {
91  vpImage<unsigned char> map(700, 300);
92  map = 0;
93  unsigned int width = map.getWidth();
94  unsigned int height = map.getHeight();
95  vpImagePoint O; // Beam origin
96  O.set_i(height);
97  O.set_j(width/2.);
98  vpScanPoint p;
99  vpColor color[4]; // one color per layer
100  char filename[FILENAME_MAX];
101  std::ofstream fdscan;
102  vpLaserScan laserscan[4];
103 
104  for (int layer=0; layer<4; layer++) {
105  switch (layer) {
106  case 0: color[layer] = vpColor::red; break;
107  case 1: color[layer] = vpColor::green; break;
108  case 2: color[layer] = vpColor::blue; break;
109  case 3: color[layer] = vpColor::yellow; break;
110  }
111  }
112 
113  vpDisplay *display;
114 #if defined VISP_HAVE_X11
115  display = new vpDisplayX;
116 #elif defined VISP_HAVE_GDI
117  display = new vpDisplayGDI;
118 #elif defined VISP_HAVE_GTK
119  display = new vpDisplayGTK;
120 #endif
121  display->init (map, 10, 10, "Laser scan");
122 
123  unsigned int iter = 0;
124  for ( ; ; ) {
125 
126 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
127  vpDisplay::display(map);
128 #endif
129 
130 #ifdef VISP_HAVE_PTHREAD
131  pthread_mutex_lock(&shm_mutex);
132 #endif
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);
137 #endif
138 
139  // std::cout << "laser start timestamp "
140  // << laserscan[0].getStartTimestamp() - time_offset << std::endl;
141 
142  // Parse the four layers
143  for (int layer=0; layer<4; layer++) {
144  if (! ((0x1<<layer) & layerToDisplay)) {
145  std::cout << "Layer " << layer+1 << " is not displayed" << std::endl;
146  continue;
147  }
148 
149  std::vector<vpScanPoint> pointsLayer = laserscan[layer].getScanPoints();
150 
151  if (save) {
152  // Set the scan data filename to store the measures
153  sprintf(filename, "%s/scan%04d-layer%d.txt",
154  output_path.c_str(), iter, layer+1);
155  fdscan.open(filename);
156 
157  // Write the file header
158  fdscan << "# Scan layer [1 to 4] : " << layer+1 << std::endl
159  << "# Start timestamp (s) : "
160  << laserscan[layer].getStartTimestamp() - time_offset
161  << std::endl
162  << "# End timestamp (s) : "
163  << laserscan[layer].getEndTimestamp() - time_offset
164  << std::endl
165  << "# Data : \"radial distance (m)\" \"horizontal angle (rad)\" \"vertical angle (rad)\" \"X (m)\" \"Y (m)\" \"Z (m)\""
166  << std::endl;
167  }
168 
169 
170  vpImagePoint E; // Beam echo
171  double resolution = 5; // 100 pixels = 1 meter - increase this value to see better near info
172  // std::cout << "display layer " << layer << " nb points: "
173  // << pointsLayer.size() << std::endl;
174  for (unsigned int i=0; i<pointsLayer.size(); i++) {
175  p = pointsLayer[i];
176  E.set_i(height - resolution * p.getRadialDist() * cos(p.getHAngle()));
177  E.set_j(width/2. - resolution * p.getRadialDist() * sin(p.getHAngle()));
178  //std::cout << "E: " << E << std::endl;
179 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
180  vpDisplay::displayLine(map, O, E, color[layer]);
181 #endif
182  if (save) {
183  // Save the measures in the file
184  fdscan << p << std::endl;
185  }
186  }
187  if (save) {
188  fdscan.close();
189  }
190  }
191 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
192  vpDisplay::flush(map);
193 #endif
194  iter ++;
195  //std::cout << "display time: " << vpTime::measureTimeMs() - t1 << std::endl;
196 
197  }
198  delete display;
199  return NULL;
200 }
201 
202 void *laser_acq_loop(void *)
203 {
204  std::string ip = "131.254.12.119";
205 
206  vpSickLDMRS laser;
207  laser.setIpAddress(ip);
208  laser.setup();
209  vpLaserScan laserscan[4];
210 
211  unsigned int iter = 0;
212  for ( ; ; ) {
213  double t1 = vpTime::measureTimeMs();
214  if (laser.measure(laserscan) == false)
215  continue;
216 
217 #ifdef VISP_HAVE_PTHREAD
218  pthread_mutex_lock(&shm_mutex);
219 #endif
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);
224 #endif
225 
226  iter ++;
227  std::cout << "laser acq time: " << vpTime::measureTimeMs() - t1 << std::endl;
228 
229  }
230 
231  return NULL;
232 }
233 
234 void *camera_acq_and_display_loop(void *)
235 {
236 #ifdef VISP_HAVE_DC1394_2
237  try {
238  // Initialize the firewire framegrabber
239  vp1394TwoGrabber g; // Create a grabber based on libdc1394-2.x third party lib
240 
241  // If no camera found return
242  if (g.getNumCameras() == 0)
243  return NULL;
244 
245  // g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
246  // g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
247 
248  vpImage<unsigned char> I; // Create a gray level image container
249  vpImage<unsigned char> Q; // Create a quarter size gray level image container
250  g.acquire(I); // Acquire an image
251  I.quarterSizeImage(Q);
252 
253  vpDisplay *display;
254 #if defined VISP_HAVE_X11
255  display = new vpDisplayX;
256 #elif defined VISP_HAVE_GDI
257  display = new vpDisplayGDI;
258 #elif defined VISP_HAVE_GTK
259  display = new vpDisplayGTK;
260 #endif
261  display->init (Q, 320, 10, "Camera");
262 
263  // Create a file with cameraimage time stamps
264  std::ofstream fdimage_ts;
265  if (save) {
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;
269  }
270  dc1394video_frame_t *frame;
271  unsigned iter = 0;
272  char filename[FILENAME_MAX];
273  uint64_t timestamp;
274  uint32_t id;
275  double image_timestamp;
276  for ( ; ; ) {
277  frame = g.dequeue(I, timestamp, id); // Acquire an image
278  I.quarterSizeImage(Q);
279  image_timestamp = timestamp/1000000. - time_offset;
280  std::cout << "camera timestamp: " << image_timestamp << " s " << std::endl;
281  if (save) {
282  // Set the image filename
283  sprintf(filename, "%s/image%04d.png", output_path.c_str(), iter);
284  vpImageIo::write(Q, filename);
285  fdimage_ts << filename << " " << image_timestamp << std::endl;
286  }
287 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
289  vpDisplay::flush(Q);
290 #endif
291  g.enqueue(frame);
292 
293  iter ++;
294  }
295  delete display;
296  if (save) {
297  fdimage_ts.close();
298  }
299  }
300  catch(...) {
301 
302  }
303 #endif
304  return NULL;
305 }
306 
307 
308 int main(int argc, const char ** argv)
309 {
310  output_path = "data";
311  // Test if the output path directory exist. If no try to create it
312  if (vpIoTools::checkDirectory(output_path) == false) {
313  try {
314  // Create a directory with name "username"
315  vpIoTools::makeDirectory(output_path);
316  }
317  catch (...) {
318  std::cout << "Cannot create " << output_path << " directory" << std::endl;
319  return false;
320  }
321  }
322 
323  // Parse the command line to set the variables
324  vpParseArgv::vpArgvInfo argTable[] =
325  {
326  {"-layer", vpParseArgv::ARGV_INT, (char*) NULL, (char *) &layerToDisplay,
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."
333  },
334  {"-save", vpParseArgv::ARGV_INT, (char*) NULL, (char *) &save,
335  "Turn to 1 in order to save data."
336  },
337  {"-h", vpParseArgv::ARGV_HELP, (char*) NULL, (char *) NULL,
338  "Display one or more measured layers form a Sick LD-MRS laser scanner."},
339  {(char*) NULL, vpParseArgv::ARGV_END, (char*) NULL, (char*) NULL, (char*) NULL}
340  } ;
341 
342  // Read the command line options
343  if(vpParseArgv::parse(&argc, argv, argTable,
347  return (false);
348  }
349 
350  time_offset = vpTime::measureTimeSecond();
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);
361 #endif
362 
363  return 0;
364 }
365 
366 #else // #ifdef UNIX and display
367 
368 int main()
369 {
370  std::cout << "This example is only working on UNIX platforms \n"
371  << "since the Sick LD-MRS driver was not ported to Windows."
372  << std::endl;
373  return 0;
374 }
375 
376 #endif // #ifdef UNIX
void set_j(const double j)
Definition: vpImagePoint.h:156
static void write(const vpImage< unsigned char > &I, const char *filename)
Definition: vpImageIo.cpp:442
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:175
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:335
static void close(vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2028
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:133
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
Define the X11 console to display images.
Definition: vpDisplayX.h:152
void set_i(const double i)
Definition: vpImagePoint.h:145
std::vector< vpScanPoint > getScanPoints()
Definition: vpLaserScan.h:101
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:1991
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:79
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:404
void quarterSizeImage(vpImage< Type > &res)
Definition: vpImage.h:943
Class that defines a single laser scanner point.
Definition: vpScanPoint.h:74
double getRadialDist() const
Definition: vpScanPoint.h:116
void getNumCameras(unsigned int &ncameras)
double getHAngle() const
Definition: vpScanPoint.h:128
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:203
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:145
void enqueue(dc1394video_frame_t *frame)
Command line argument parsing.
Definition: vpParseArgv.h:133
double getStartTimestamp()
Definition: vpLaserScan.h:134
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:92
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:138