Visual Servoing Platform version 3.5.0
SickLDMRS-Process.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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/vpDisplayGTK.h>
68#include <visp3/gui/vpDisplayX.h>
69#endif
70#include <visp3/core/vpIoTools.h>
71#include <visp3/io/vpParseArgv.h>
72#include <visp3/sensor/vp1394TwoGrabber.h>
73
74#if (!defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))) && \
75 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK))
76
77static int save = 0;
78static int layerToDisplay = 0xF; // 0xF = 1111 => all the layers are selected
79static vpLaserScan shm_laserscan[4];
80double time_offset = 0;
81#ifdef VISP_HAVE_PTHREAD
82pthread_mutex_t shm_mutex;
83#endif
84std::string output_path;
85
86void *laser_display_and_save_loop(void *);
87void *laser_acq_loop(void *);
88void *camera_acq_and_display_loop(void *);
89
90void *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.);
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:
108 color[layer] = vpColor::red;
109 break;
110 case 1:
111 color[layer] = vpColor::green;
112 break;
113 case 2:
114 color[layer] = vpColor::blue;
115 break;
116 case 3:
117 color[layer] = vpColor::yellow;
118 break;
119 }
120 }
121
122 vpDisplay *display = NULL;
123#ifdef VISP_HAVE_MODULE_GUI
124#if defined VISP_HAVE_X11
125 display = new vpDisplayX;
126#elif defined VISP_HAVE_GTK
127 display = new vpDisplayGTK;
128#endif
129 display->init(map, 10, 10, "Laser scan");
130#endif
131
132 unsigned int iter = 0;
133 for (;;) {
134
135#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
137#endif
138
139#ifdef VISP_HAVE_PTHREAD
140 pthread_mutex_lock(&shm_mutex);
141#endif
142 for (int layer = 0; layer < 4; layer++)
143 laserscan[layer] = shm_laserscan[layer];
144#ifdef VISP_HAVE_PTHREAD
145 pthread_mutex_unlock(&shm_mutex);
146#endif
147
148 // std::cout << "laser start timestamp "
149 // << laserscan[0].getStartTimestamp() - time_offset << std::endl;
150
151 // Parse the four layers
152 for (int layer = 0; layer < 4; layer++) {
153 if (!((0x1 << layer) & layerToDisplay)) {
154 std::cout << "Layer " << layer + 1 << " is not displayed" << std::endl;
155 continue;
156 }
157
158 std::vector<vpScanPoint> pointsLayer = laserscan[layer].getScanPoints();
159
160 if (save) {
161 // Set the scan data filename to store the measures
162 sprintf(filename, "%s/scan%04u-layer%d.txt", output_path.c_str(), iter, layer + 1);
163 fdscan.open(filename);
164
165 // Write the file header
166 fdscan << "# Scan layer [1 to 4] : " << layer + 1 << std::endl
167 << "# Start timestamp (s) : " << laserscan[layer].getStartTimestamp() - time_offset << std::endl
168 << "# End timestamp (s) : " << laserscan[layer].getEndTimestamp() - time_offset << std::endl
169 << "# Data : \"radial distance (m)\" \"horizontal angle "
170 "(rad)\" \"vertical angle (rad)\" \"X (m)\" \"Y (m)\" \"Z "
171 "(m)\""
172 << std::endl;
173 }
174
175 vpImagePoint E; // Beam echo
176 double resolution = 5; // 100 pixels = 1 meter - increase this value to
177 // see better near info
178 // std::cout << "display layer " << layer << " nb points: "
179 // << pointsLayer.size() << std::endl;
180 for (unsigned int i = 0; i < pointsLayer.size(); i++) {
181 p = pointsLayer[i];
182 E.set_i(height - resolution * p.getRadialDist() * cos(p.getHAngle()));
183 E.set_j(width / 2. - resolution * p.getRadialDist() * sin(p.getHAngle()));
184// std::cout << "E: " << E << std::endl;
185#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
186 vpDisplay::displayLine(map, O, E, color[layer]);
187#endif
188 if (save) {
189 // Save the measures in the file
190 fdscan << p << std::endl;
191 }
192 }
193 if (save) {
194 fdscan.close();
195 }
196 }
197#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
198 vpDisplay::flush(map);
199#endif
200 iter++;
201 // std::cout << "display time: " << vpTime::measureTimeMs() - t1 <<
202 // std::endl;
203 }
204 delete display;
205 return NULL;
206}
207
208void *laser_acq_loop(void *)
209{
210 std::string ip = "131.254.12.119";
211
212 vpSickLDMRS laser;
213 laser.setIpAddress(ip);
214 laser.setup();
215 vpLaserScan laserscan[4];
216
217 unsigned int iter = 0;
218 for (;;) {
219 double t1 = vpTime::measureTimeMs();
220 if (laser.measure(laserscan) == false)
221 continue;
222
223#ifdef VISP_HAVE_PTHREAD
224 pthread_mutex_lock(&shm_mutex);
225#endif
226 for (int layer = 0; layer < 4; layer++)
227 shm_laserscan[layer] = laserscan[layer];
228#ifdef VISP_HAVE_PTHREAD
229 pthread_mutex_unlock(&shm_mutex);
230#endif
231
232 iter++;
233 std::cout << "laser acq time: " << vpTime::measureTimeMs() - t1 << std::endl;
234 }
235
236 return NULL;
237}
238
239void *camera_acq_and_display_loop(void *)
240{
241#ifdef VISP_HAVE_DC1394
242 try {
243 // Initialize the firewire framegrabber
244 vp1394TwoGrabber g; // Create a grabber based on libdc1394-2.x third party lib
245
246 // If no camera found return
247 if (g.getNumCameras() == 0)
248 return NULL;
249
250 // g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
251 // g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
252
253 vpImage<unsigned char> I; // Create a gray level image container
254 vpImage<unsigned char> Q; // Create a quarter size gray level image container
255 g.acquire(I); // Acquire an image
256 I.quarterSizeImage(Q);
257
258 vpDisplay *display = NULL;
259#ifdef VISP_HAVE_MODULE_GUI
260#if defined VISP_HAVE_X11
261 display = new vpDisplayX;
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))
293#endif
294 g.enqueue(frame);
295
296 iter++;
297 }
298 delete display;
299 if (save) {
300 fdimage_ts.close();
301 }
302 } catch (...) {
303 }
304#endif
305 return NULL;
306}
307
308int main(int argc, const char **argv)
309{
310 try {
311 output_path = "data";
312 // Test if the output path directory exist. If no try to create it
313 if (vpIoTools::checkDirectory(output_path) == false) {
314 try {
315 // Create a directory with name "username"
316 vpIoTools::makeDirectory(output_path);
317 } catch (...) {
318 std::cout << "Cannot create " << output_path << " directory" << std::endl;
319 return EXIT_FAILURE;
320 }
321 }
322
323 // Parse the command line to set the variables
324 vpParseArgv::vpArgvInfo argTable[] = {
325 {"-layer", vpParseArgv::ARGV_INT, (char *)NULL, (char *)&layerToDisplay,
326 "The layer to display:\n"
327 "\t\t. 0x1 for layer 1.\n"
328 "\t\t. 0x2 for layer 2.\n"
329 "\t\t. 0x4 for layer 3.\n"
330 "\t\t. 0x8 for layer 4.\n"
331 "\t\tTo display all the layers you should set 0xF value."},
332 {"-save", vpParseArgv::ARGV_INT, (char *)NULL, (char *)&save, "Turn to 1 in order to save data."},
333 {"-h", vpParseArgv::ARGV_HELP, (char *)NULL, (char *)NULL,
334 "Display one or more measured layers form a Sick LD-MRS laser "
335 "scanner."},
336 {(char *)NULL, vpParseArgv::ARGV_END, (char *)NULL, (char *)NULL, (char *)NULL}};
337
338 // Read the command line options
339 if (vpParseArgv::parse(&argc, argv, argTable,
342 return (EXIT_FAILURE);
343 }
344
345 time_offset = vpTime::measureTimeSecond();
346#ifdef VISP_HAVE_PTHREAD
347 pthread_t thread_camera_acq;
348 pthread_t thread_laser_acq;
349 pthread_t thread_laser_display;
350 pthread_create(&thread_camera_acq, NULL, &camera_acq_and_display_loop, NULL);
351 pthread_create(&thread_laser_acq, NULL, &laser_acq_loop, NULL);
352 pthread_create(&thread_laser_display, NULL, &laser_display_and_save_loop, NULL);
353 pthread_join(thread_camera_acq, 0);
354 pthread_join(thread_laser_acq, 0);
355 pthread_join(thread_laser_display, 0);
356#endif
357
358 return EXIT_SUCCESS;
359 } catch (const vpException &e) {
360 std::cout << "Catch an exception: " << e << std::endl;
361 return EXIT_FAILURE;
362 }
363}
364
365#elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK))
366int main()
367{
368 std::cout << "You do not have X11, or GTK functionalities to display images..." << std::endl;
369 std::cout << "Tip if you are on a unix-like system:" << std::endl;
370 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
371 std::cout << "Tip if you are on a windows-like system:" << std::endl;
372 std::cout << "- Install GTK, configure again ViSP using cmake and build again this example" << std::endl;
373 return EXIT_SUCCESS;
374}
375#else // #ifdef UNIX and display
376
377int main()
378{
379 std::cout << "This example is only working on unix-like platforms \n"
380 << "since the Sick LD-MRS driver was not ported to Windows." << std::endl;
381 return EXIT_SUCCESS;
382}
383
384#endif // #ifdef UNIX
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void acquire(vpImage< unsigned char > &I)
void enqueue(dc1394video_frame_t *frame)
dc1394video_frame_t * dequeue()
void getNumCameras(unsigned int &ncameras) const
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static const vpColor blue
Definition: vpColor.h:223
static const vpColor yellow
Definition: vpColor.h:225
static const vpColor green
Definition: vpColor.h:220
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:135
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:178
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void flush(const vpImage< unsigned char > &I)
error that can be emited by ViSP classes.
Definition: vpException.h:72
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:293
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void set_j(double jj)
Definition: vpImagePoint.h:177
void set_i(double ii)
Definition: vpImagePoint.h:166
void quarterSizeImage(vpImage< Type > &res) const
Definition: vpImage.h:1256
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:420
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:570
Implements a laser scan data structure that contains especially the list of scanned points that have ...
Definition: vpLaserScan.h:65
double getStartTimestamp()
Definition: vpLaserScan.h:116
std::vector< vpScanPoint > getScanPoints()
Definition: vpLaserScan.h:94
double getEndTimestamp()
Definition: vpLaserScan.h:118
void setIpAddress(std::string ip_address)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
@ ARGV_NO_DEFAULTS
No default options like -help.
Definition: vpParseArgv.h:173
@ ARGV_NO_ABBREV
No abrevation. Print an error message if an option is abrevated (ie "-i" in place of "-int" which is ...
Definition: vpParseArgv.h:175
@ ARGV_NO_LEFTOVERS
Print an error message if an option is not in the argument list.
Definition: vpParseArgv.h:174
@ ARGV_INT
Argument is associated to an int.
Definition: vpParseArgv.h:157
@ ARGV_END
End of the argument list.
Definition: vpParseArgv.h:166
@ ARGV_HELP
Argument is for help displaying.
Definition: vpParseArgv.h:165
Class that defines a single laser scanner point.
Definition: vpScanPoint.h:76
double getRadialDist() const
Definition: vpScanPoint.h:116
double getHAngle() const
Definition: vpScanPoint.h:124
Driver for the Sick LD-MRS laser scanner.
Definition: vpSickLDMRS.h:108
bool setup(const std::string &ip, int port)
bool measure(vpLaserScan laserscan[4])
VISP_EXPORT double measureTimeSecond()
VISP_EXPORT double measureTimeMs()