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(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) ) && (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;
88 void *laser_display_and_save_loop(
void *);
89 void *laser_acq_loop(
void *);
90 void *camera_acq_and_display_loop(
void *);
92 void *laser_display_and_save_loop(
void *)
96 unsigned int width = map.getWidth();
97 unsigned int height = map.getHeight();
103 char filename[FILENAME_MAX];
104 std::ofstream fdscan;
107 for (
int layer=0; layer<4; layer++) {
117 #if defined VISP_HAVE_X11
119 #elif defined VISP_HAVE_GDI
121 #elif defined VISP_HAVE_GTK
124 display->
init (map, 10, 10,
"Laser scan");
126 unsigned int iter = 0;
129 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
133 #ifdef VISP_HAVE_PTHREAD
134 pthread_mutex_lock(&shm_mutex);
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);
146 for (
int layer=0; layer<4; layer++) {
147 if (! ((0x1<<layer) & layerToDisplay)) {
148 std::cout <<
"Layer " << layer+1 <<
" is not displayed" << std::endl;
152 std::vector<vpScanPoint> pointsLayer = laserscan[layer].
getScanPoints();
156 sprintf(filename,
"%s/scan%04d-layer%d.txt",
157 output_path.c_str(), iter, layer+1);
158 fdscan.open(filename);
161 fdscan <<
"# Scan layer [1 to 4] : " << layer+1 << std::endl
162 <<
"# Start timestamp (s) : "
165 <<
"# End timestamp (s) : "
168 <<
"# Data : \"radial distance (m)\" \"horizontal angle (rad)\" \"vertical angle (rad)\" \"X (m)\" \"Y (m)\" \"Z (m)\""
174 double resolution = 5;
177 for (
unsigned int i=0; i<pointsLayer.size(); i++) {
182 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
187 fdscan << p << std::endl;
194 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
205 void *laser_acq_loop(
void *)
207 std::string ip =
"131.254.12.119";
210 laser.setIpAddress(ip);
214 unsigned int iter = 0;
217 if (laser.measure(laserscan) ==
false)
220 #ifdef VISP_HAVE_PTHREAD
221 pthread_mutex_lock(&shm_mutex);
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);
237 void *camera_acq_and_display_loop(
void *)
239 #ifdef VISP_HAVE_DC1394_2
257 #if defined VISP_HAVE_X11
259 #elif defined VISP_HAVE_GDI
261 #elif defined VISP_HAVE_GTK
264 display->
init (Q, 320, 10,
"Camera");
267 std::ofstream fdimage_ts;
269 std::string filename = output_path +
"/image_timestamp.txt";
270 fdimage_ts.open( filename.c_str() );
271 fdimage_ts <<
"# [image name] [time stamp in second]" << std::endl;
273 dc1394video_frame_t *frame;
275 char filename[FILENAME_MAX];
278 double image_timestamp;
280 frame = g.
dequeue(I, timestamp,
id);
282 image_timestamp = timestamp/1000000. - time_offset;
283 std::cout <<
"camera timestamp: " << image_timestamp <<
" s " << std::endl;
286 sprintf(filename,
"%s/image%04d.png", output_path.c_str(), iter);
288 fdimage_ts << filename <<
" " << image_timestamp << std::endl;
290 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
311 int main(
int argc,
const char ** argv)
314 output_path =
"data";
322 std::cout <<
"Cannot create " << output_path <<
" directory" << std::endl;
328 vpParseArgv::vpArgvInfo argTable[] =
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."
339 "Turn to 1 in order to save data."
342 "Display one or more measured layers form a Sick LD-MRS laser scanner."},
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);
370 std::cout <<
"Catch an exception: " << e << std::endl;
375 #else // #ifdef UNIX and display
379 std::cout <<
"This example is only working on UNIX platforms \n"
380 <<
"since the Sick LD-MRS driver was not ported to Windows."
385 #endif // #ifdef UNIX
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.
error that can be emited by ViSP classes.
std::vector< vpScanPoint > getScanPoints()
void getNumCameras(unsigned int &ncameras) const
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)
void set_i(const double ii)
Class that defines a single laser scanner point.
double getRadialDist() const
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)
void set_j(const double jj)
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