64 #include <visp/vpDebug.h>
65 #include <visp/vpImagePoint.h>
66 #include <visp/vpSickLDMRS.h>
67 #include <visp/vpImage.h>
68 #include <visp/vpImageIo.h>
69 #include <visp/vpDisplay.h>
70 #include <visp/vpDisplayX.h>
71 #include <visp/vpDisplayGDI.h>
72 #include <visp/vpDisplayGTK.h>
73 #include <visp/vpParseArgv.h>
74 #include <visp/vp1394TwoGrabber.h>
75 #include <visp/vpIoTools.h>
77 #if ( defined(UNIX) && ( ! defined(WIN32) ) ) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
80 static int layerToDisplay = 0xF;
82 double time_offset = 0;
83 #ifdef VISP_HAVE_PTHREAD
84 pthread_mutex_t shm_mutex;
86 std::string output_path;
89 void *laser_display_and_save_loop(
void *)
93 unsigned int width = map.getWidth();
94 unsigned int height = map.getHeight();
100 char filename[FILENAME_MAX];
101 std::ofstream fdscan;
104 for (
int layer=0; layer<4; layer++) {
114 #if defined VISP_HAVE_X11
116 #elif defined VISP_HAVE_GDI
118 #elif defined VISP_HAVE_GTK
121 display->
init (map, 10, 10,
"Laser scan");
123 unsigned int iter = 0;
126 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
130 #ifdef VISP_HAVE_PTHREAD
131 pthread_mutex_lock(&shm_mutex);
133 for (
int layer=0; layer < 4; layer++)
134 laserscan[layer] = shm_laserscan[layer];
135 #ifdef VISP_HAVE_PTHREAD
136 pthread_mutex_unlock(&shm_mutex);
143 for (
int layer=0; layer<4; layer++) {
144 if (! ((0x1<<layer) & layerToDisplay)) {
145 std::cout <<
"Layer " << layer+1 <<
" is not displayed" << std::endl;
149 std::vector<vpScanPoint> pointsLayer = laserscan[layer].
getScanPoints();
153 sprintf(filename,
"%s/scan%04d-layer%d.txt",
154 output_path.c_str(), iter, layer+1);
155 fdscan.open(filename);
158 fdscan <<
"# Scan layer [1 to 4] : " << layer+1 << std::endl
159 <<
"# Start timestamp (s) : "
162 <<
"# End timestamp (s) : "
165 <<
"# Data : \"radial distance (m)\" \"horizontal angle (rad)\" \"vertical angle (rad)\" \"X (m)\" \"Y (m)\" \"Z (m)\""
171 double resolution = 5;
174 for (
unsigned int i=0; i<pointsLayer.size(); i++) {
179 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
184 fdscan << p << std::endl;
191 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
202 void *laser_acq_loop(
void *)
204 std::string ip =
"131.254.12.119";
207 laser.setIpAddress(ip);
211 unsigned int iter = 0;
214 if (laser.measure(laserscan) ==
false)
217 #ifdef VISP_HAVE_PTHREAD
218 pthread_mutex_lock(&shm_mutex);
220 for (
int layer=0; layer < 4; layer++)
221 shm_laserscan[layer] = laserscan[layer];
222 #ifdef VISP_HAVE_PTHREAD
223 pthread_mutex_unlock(&shm_mutex);
234 void *camera_acq_and_display_loop(
void *)
236 #ifdef VISP_HAVE_DC1394_2
254 #if defined VISP_HAVE_X11
256 #elif defined VISP_HAVE_GDI
258 #elif defined VISP_HAVE_GTK
261 display->
init (Q, 320, 10,
"Camera");
264 std::ofstream fdimage_ts;
266 std::string filename = output_path +
"/image_timestamp.txt";
267 fdimage_ts.open( filename.c_str() );
268 fdimage_ts <<
"# [image name] [time stamp in second]" << std::endl;
270 dc1394video_frame_t *frame;
272 char filename[FILENAME_MAX];
275 double image_timestamp;
277 frame = g.
dequeue(I, timestamp,
id);
279 image_timestamp = timestamp/1000000. - time_offset;
280 std::cout <<
"camera timestamp: " << image_timestamp <<
" s " << std::endl;
283 sprintf(filename,
"%s/image%04d.png", output_path.c_str(), iter);
285 fdimage_ts << filename <<
" " << image_timestamp << std::endl;
287 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
308 int main(
int argc,
const char ** argv)
310 output_path =
"data";
318 std::cout <<
"Cannot create " << output_path <<
" directory" << std::endl;
324 vpParseArgv::vpArgvInfo argTable[] =
327 "The layer to display:\n"
328 "\t\t. 0x1 for layer 1.\n"
329 "\t\t. 0x2 for layer 2.\n"
330 "\t\t. 0x4 for layer 3.\n"
331 "\t\t. 0x8 for layer 4.\n"
332 "\t\tTo display all the layers you should set 0xF value."
335 "Turn to 1 in order to save data."
338 "Display one or more measured layers form a Sick LD-MRS laser scanner."},
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);
366 #else // #ifdef UNIX and display
370 std::cout <<
"This example is only working on UNIX platforms \n"
371 <<
"since the Sick LD-MRS driver was not ported to Windows."
376 #endif // #ifdef UNIX
void set_j(const double j)
static void write(const vpImage< unsigned char > &I, const char *filename)
virtual void init(vpImage< unsigned char > &I, int x=-1, int y=-1, const char *title=NULL)=0
Class that defines generic functionnalities for display.
static void close(vpImage< unsigned char > &I)
Display for windows using GDI (available on any windows 32 platform).
Class to define colors available for display functionnalities.
Define the X11 console to display images.
void set_i(const double i)
std::vector< vpScanPoint > getScanPoints()
static double measureTimeMs()
static const vpColor green
void acquire(vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Implements a laser scan data structure that contains especially the list of scanned points that have ...
void quarterSizeImage(vpImage< Type > &res)
Class that defines a single laser scanner point.
double getRadialDist() const
void getNumCameras(unsigned int &ncameras)
static void display(const vpImage< unsigned char > &I)
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
void enqueue(dc1394video_frame_t *frame)
Command line argument parsing.
double getStartTimestamp()
dc1394video_frame_t * dequeue()
Class for firewire ieee1394 video devices using libdc1394-2.x api.
static double measureTimeSecond()
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
static const vpColor yellow
static const vpColor blue