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