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