Visual Servoing Platform  version 3.0.1
SickLDMRS-Process.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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%04u-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  unsigned iter = 0;
276  char filename[FILENAME_MAX];
277  uint64_t timestamp;
278  uint32_t id;
279  for ( ; ; ) {
280  dc1394video_frame_t *frame = g.dequeue(I, timestamp, id); // Acquire an image
281  I.quarterSizeImage(Q);
282  double 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%04u.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
void getNumCameras(unsigned int &ncameras) const
bool setup(std::string ip, int port)
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:169
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:358
static void close(vpImage< unsigned char > &I)
double getRadialDist() const
Definition: vpScanPoint.h:112
VISP_EXPORT double measureTimeSecond()
Definition: vpTime.cpp:255
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
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:153
error that can be emited by ViSP classes.
Definition: vpException.h:73
double getHAngle() const
Definition: vpScanPoint.h:124
std::vector< vpScanPoint > getScanPoints()
Definition: vpLaserScan.h:97
static const vpColor green
Definition: vpColor.h:166
void acquire(vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
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 write(const vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:368
static void makeDirectory(const char *dirname)
Definition: vpIoTools.cpp:427
void set_i(const double ii)
Definition: vpImagePoint.h:163
Class that defines a single laser scanner point.
Definition: vpScanPoint.h:72
static void display(const vpImage< unsigned char > &I)
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:138
void enqueue(dc1394video_frame_t *frame)
void quarterSizeImage(vpImage< Type > &res) const
Definition: vpImage.h:1374
void set_j(const double jj)
Definition: vpImagePoint.h:174
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
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
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