ViSP  3.0.0
SickLDMRS-Process.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Sick LD-MRS laser driver.
32  *
33  * Authors:
34  * Fabien Spindler
35  *
36  *****************************************************************************/
37 
38 
60 #include <visp3/core/vpDebug.h>
61 #include <visp3/core/vpImagePoint.h>
62 #include <visp3/sensor/vpSickLDMRS.h>
63 #include <visp3/core/vpImage.h>
64 #include <visp3/io/vpImageIo.h>
65 #include <visp3/core/vpDisplay.h>
66 #ifdef VISP_HAVE_MODULE_GUI
67 # include <visp3/gui/vpDisplayX.h>
68 # include <visp3/gui/vpDisplayGDI.h>
69 # include <visp3/gui/vpDisplayGTK.h>
70 #endif
71 #include <visp3/io/vpParseArgv.h>
72 #include <visp3/sensor/vp1394TwoGrabber.h>
73 #include <visp3/core/vpIoTools.h>
74 
75 #if ( !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) ) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
76 
77 static int save = 0;
78 static int layerToDisplay = 0xF; // 0xF = 1111 => all the layers are selected
79 static vpLaserScan shm_laserscan[4];
80 double time_offset = 0;
81 #ifdef VISP_HAVE_PTHREAD
82 pthread_mutex_t shm_mutex;
83 #endif
84 std::string output_path;
85 
86 void *laser_display_and_save_loop(void *);
87 void *laser_acq_loop(void *);
88 void *camera_acq_and_display_loop(void *);
89 
90 void *laser_display_and_save_loop(void *)
91 {
92  vpImage<unsigned char> map(700, 300);
93  map = 0;
94  unsigned int width = map.getWidth();
95  unsigned int height = map.getHeight();
96  vpImagePoint O; // Beam origin
97  O.set_i(height);
98  O.set_j(width/2.);
99  vpScanPoint p;
100  vpColor color[4]; // one color per layer
101  char filename[FILENAME_MAX];
102  std::ofstream fdscan;
103  vpLaserScan laserscan[4];
104 
105  for (int layer=0; layer<4; layer++) {
106  switch (layer) {
107  case 0: color[layer] = vpColor::red; break;
108  case 1: color[layer] = vpColor::green; break;
109  case 2: color[layer] = vpColor::blue; break;
110  case 3: color[layer] = vpColor::yellow; break;
111  }
112  }
113 
114  vpDisplay *display = NULL;
115 #ifdef VISP_HAVE_MODULE_GUI
116 #if defined VISP_HAVE_X11
117  display = new vpDisplayX;
118 #elif defined VISP_HAVE_GDI
119  display = new vpDisplayGDI;
120 #elif defined VISP_HAVE_GTK
121  display = new vpDisplayGTK;
122 #endif
123  display->init (map, 10, 10, "Laser scan");
124 #endif
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
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 = NULL;
257 #ifdef VISP_HAVE_MODULE_GUI
258 #if defined VISP_HAVE_X11
259  display = new vpDisplayX;
260 #elif defined VISP_HAVE_GDI
261  display = new vpDisplayGDI;
262 #elif defined VISP_HAVE_GTK
263  display = new vpDisplayGTK;
264 #endif
265  display->init (Q, 320, 10, "Camera");
266 #endif
267 
268  // Create a file with cameraimage time stamps
269  std::ofstream fdimage_ts;
270  if (save) {
271  std::string filename = output_path + "/image_timestamp.txt";
272  fdimage_ts.open( filename.c_str() );
273  fdimage_ts << "# [image name] [time stamp in second]" << std::endl;
274  }
275  dc1394video_frame_t *frame;
276  unsigned iter = 0;
277  char filename[FILENAME_MAX];
278  uint64_t timestamp;
279  uint32_t id;
280  double image_timestamp;
281  for ( ; ; ) {
282  frame = g.dequeue(I, timestamp, id); // Acquire an image
283  I.quarterSizeImage(Q);
284  image_timestamp = timestamp/1000000. - time_offset;
285  std::cout << "camera timestamp: " << image_timestamp << " s " << std::endl;
286  if (save) {
287  // Set the image filename
288  sprintf(filename, "%s/image%04d.png", output_path.c_str(), iter);
289  vpImageIo::write(Q, filename);
290  fdimage_ts << filename << " " << image_timestamp << std::endl;
291  }
292 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
294  vpDisplay::flush(Q);
295 #endif
296  g.enqueue(frame);
297 
298  iter ++;
299  }
300  delete display;
301  if (save) {
302  fdimage_ts.close();
303  }
304  }
305  catch(...) {
306 
307  }
308 #endif
309  return NULL;
310 }
311 
312 
313 int main(int argc, const char ** argv)
314 {
315  try {
316  output_path = "data";
317  // Test if the output path directory exist. If no try to create it
318  if (vpIoTools::checkDirectory(output_path) == false) {
319  try {
320  // Create a directory with name "username"
321  vpIoTools::makeDirectory(output_path);
322  }
323  catch (...) {
324  std::cout << "Cannot create " << output_path << " directory" << std::endl;
325  return false;
326  }
327  }
328 
329  // Parse the command line to set the variables
330  vpParseArgv::vpArgvInfo argTable[] =
331  {
332  {"-layer", vpParseArgv::ARGV_INT, (char*) NULL, (char *) &layerToDisplay,
333  "The layer to display:\n"
334  "\t\t. 0x1 for layer 1.\n"
335  "\t\t. 0x2 for layer 2.\n"
336  "\t\t. 0x4 for layer 3.\n"
337  "\t\t. 0x8 for layer 4.\n"
338  "\t\tTo display all the layers you should set 0xF value."
339  },
340  {"-save", vpParseArgv::ARGV_INT, (char*) NULL, (char *) &save,
341  "Turn to 1 in order to save data."
342  },
343  {"-h", vpParseArgv::ARGV_HELP, (char*) NULL, (char *) NULL,
344  "Display one or more measured layers form a Sick LD-MRS laser scanner."},
345  {(char*) NULL, vpParseArgv::ARGV_END, (char*) NULL, (char*) NULL, (char*) NULL}
346  } ;
347 
348  // Read the command line options
349  if(vpParseArgv::parse(&argc, argv, argTable,
353  return (false);
354  }
355 
356  time_offset = vpTime::measureTimeSecond();
357 #ifdef VISP_HAVE_PTHREAD
358  pthread_t thread_camera_acq;
359  pthread_t thread_laser_acq;
360  pthread_t thread_laser_display;
361  pthread_create(&thread_camera_acq, NULL, &camera_acq_and_display_loop, NULL);
362  pthread_create(&thread_laser_acq, NULL, &laser_acq_loop, NULL);
363  pthread_create(&thread_laser_display, NULL, &laser_display_and_save_loop, NULL);
364  pthread_join(thread_camera_acq, 0);
365  pthread_join(thread_laser_acq, 0);
366  pthread_join(thread_laser_display, 0);
367 #endif
368 
369  return 0;
370  }
371  catch(vpException e) {
372  std::cout << "Catch an exception: " << e << std::endl;
373  return 1;
374  }
375 }
376 
377 #else // #ifdef UNIX and display
378 
379 int main()
380 {
381  std::cout << "This example is only working on UNIX platforms \n"
382  << "since the Sick LD-MRS driver was not ported to Windows."
383  << std::endl;
384  return 0;
385 }
386 
387 #endif // #ifdef UNIX
static void write(const vpImage< unsigned char > &I, const char *filename)
Definition: vpImageIo.cpp:472
virtual void init(vpImage< unsigned char > &I, int x=-1, int y=-1, const char *title=NULL)=0
bool setup(std::string ip, int port)
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:170
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:335
static void close(vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2270
VISP_EXPORT double measureTimeSecond()
Definition: vpTime.cpp:225
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
Define the X11 console to display images.
Definition: vpDisplayX.h:148
error that can be emited by ViSP classes.
Definition: vpException.h:73
std::vector< vpScanPoint > getScanPoints()
Definition: vpLaserScan.h:97
void getNumCameras(unsigned int &ncameras) const
static const vpColor green
Definition: vpColor.h:166
void acquire(vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2233
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:76
static const vpColor red
Definition: vpColor.h:163
Implements a laser scan data structure that contains especially the list of scanned points that have ...
Definition: vpLaserScan.h:63
static void makeDirectory(const char *dirname)
Definition: vpIoTools.cpp:404
void quarterSizeImage(vpImage< Type > &res)
Definition: vpImage.h:1052
void set_i(const double ii)
Definition: vpImagePoint.h:154
Class that defines a single laser scanner point.
Definition: vpScanPoint.h:72
double getRadialDist() const
Definition: vpScanPoint.h:112
double getHAngle() const
Definition: vpScanPoint.h:124
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:206
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:141
void enqueue(dc1394video_frame_t *frame)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
void set_j(const double jj)
Definition: vpImagePoint.h:165
Command line argument parsing.
Definition: vpParseArgv.h:132
double getStartTimestamp()
Definition: vpLaserScan.h:130
Driver for the Sick LD-MRS laser scanner.
Definition: vpSickLDMRS.h:106
dc1394video_frame_t * dequeue()
bool measure(vpLaserScan laserscan[4])
Class for firewire ieee1394 video devices using libdc1394-2.x api.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
static const vpColor yellow
Definition: vpColor.h:171
void setIpAddress(std::string ip_address)
static const vpColor blue
Definition: vpColor.h:169
double getEndTimestamp()
Definition: vpLaserScan.h:134